﻿/*
****************************************************************************************************************
*= = 版权信息：Copyright (C) 356 - 999. Ombres Industry Empiral. All rights reserved.
*= = 文件名称：Orcinus.c
*= = 功能描述：工业系统电力协议定义主文件
*= = 编写人员：Carlyon W. Tung
****************************************************************************************************************
*/

/*
*===============================================================================================================
*= = 声明文件
*===============================================================================================================
*/

#include <stdio.h>

#ifdef __INSIGHT_OS_WINDOWS__
#include <io.h>
#include <process.h>
#include <WinSock2.h>
#include <WS2tcpip.h>
#else
#include <dirent.h>
#include <sys/time.h>
#include <unistd.h>
#endif // End of __INSIGHT_OS_WINDOWS__

#include "Orcinus.h"

/*
*===============================================================================================================
*= = 预处理声明
*===============================================================================================================
*/

#ifdef HAVE_CONFIG_H
    #include <config.h>
#endif // End of HAVE_CONFIG_H

/*
*===============================================================================================================
*= = 宏定义
*===============================================================================================================
*/

/* Debugging */
// #define IEC_DEBUG_EVENT      /* Debug events */
// #define IEC_DEBUG_FRAME      /* Debug I/O frames */

/* Events to log */
#define IEC_LOG_FATAL       0   /* Log only fatal errors */
#define IEC_LOG_ERROR       1   /* + Log socket and IEC errors */
#define IEC_LOG_EVENT       2   /* + Log socket events */
#define IEC_LOG_WARNING     3   /* + Log IEC warnigns */
#define IEC_LOG_ALL         4   /* Log all events */

/* I/O frames to log, flags could be combined with bitwise OR. */
#define IEC_LOG_U           1   /* Log U-frames */
#define IEC_LOG_S           2   /* Log S-frames */
#define IEC_LOG_I           4   /* Log I-frames */

/* Debugging levels */
#define IEC_LOG_LEVELS      IEC_LOG_WARNING
#define IEC_LOG_LEVELA      IEC_LOG_U | IEC_LOG_S | IEC_LOG_I

/* Log event time with milliseconds */
#define IEC_LOG_TIME_MS

#define APDU_START          0x68
#define APDU_MIN_LEN        4
#define APDU_MAX_LEN        253
#define ASDU_MAX_LEN        (APDU_MAX_LEN - 4)
#define ASDU_MAX_DATA_LEN   (ASDU_MAX_LEN - 6)

/* Maximum IOA value for data indexing */
#define MAX_INDEXED_IOA     1024

#ifdef  __ORCINUS_AS_SERVICE__
    #ifdef  IEC_LOG_LEVELS
        #undef  IEC_LOG_LEVELS
        #define IEC_LOG_LEVELS       IEC_LOG_FATAL
    #endif
    #ifdef  IEC_DEBUG_FRAME
        #undef  IEC_DEBUG_FRAME
    #endif
#endif

/* Inner socket flags */
#define _IECF_S_IPV6        0x01
#define _IECF_S_CLIENT      0x02
#define _IECF_S_MASTER      0x04
#define _IECF_S_IC_REQ      0x10

#ifdef __HAVE_SSL_SUPPORT__
#define _IECF_S_SSL         0x20
#endif

#define _IECF_S_CA_LEN      0x100
#define _IECF_S_IOA_LEN     0x600
#define _IECF_WITH_TIME     0x01    /* Inner data flag */

#define IN_BUF_SIZE         8192
#define OUT_BUF_SIZE        8192
#define QUEUE_SIZE          4096

#define STORE_DATA_NUM      1024
#define ASSERT(cond, code)  if (cond) {errno = code; goto error;}
#define ASSERT1(cond, code) if (cond) {errno = code; goto error1;}

/* Get ptr to sector structure by CA */
#define IEC_SECTOR(arg) (arg < 1 || arg > icb.sector_max ? NULL : icb.sectors[arg - 1])
/* Get ptr to socket structure by socket ID */
#define IEC_SOCKET(arg) (arg < 0 || arg >= icb.socket_count ? NULL : icb.sockets[arg])
/* Is common address ASDU broadcast? */
#define CA_BROADCAST(s) (s->is.ca_len == 1 ? 0xff : 0xffff)
#define IS_CA_BROADCAST(s, ca)  (ca == CA_BROADCAST(s) || /*???*/ ca == 0)

static const char* value_type_names[] = {
    "unknown", "single point", "double point", "step position", "bitstring",
    "normalized", "scaled", "floating point", "integrated total"
};

#ifdef __INSIGHT_OS_WINDOWS__
#define GET_TYPE_NAME(arg) (arg < 1 || arg > 8 ? value_type_names[0] : value_type_names[arg])
#define SET_ERROR(fmt, ...) sprintf(icb.errormsg, fmt, __VA_ARGS__); LOG(IEC_LOG_FATAL, fmt, __VA_ARGS__)
#define EVENT(fmt, ...) if (icb.iec_event_listener) {sprintf(icb.event, fmt, __VA_ARGS__); icb.iec_event_listener(icb.event);} LOG(IEC_LOG_EVENT, fmt, __VA_ARGS__)
#else
#define GET_TYPE_NAME(arg) (arg < 1 || arg > 8 ? value_type_names[0] : value_type_names[arg])
#define SET_ERROR(fmt, ...) sprintf(icb.errormsg, fmt); LOG(IEC_LOG_FATAL, fmt)
#define EVENT(fmt, ...) if (icb.iec_event_listener) {sprintf(icb.event, fmt); icb.iec_event_listener(icb.event);} LOG(IEC_LOG_EVENT, fmt)
#endif

#define IEC_GET_MS(arg)     ((arg->flags & _IECF_S_MASTER) ? 'M' : 'S')
#define IEC_GET_IPV(arg)    ((arg->flags & _IECF_S_IPV6) ? '6' : '4')

#define ASDU_INVALID_CA         -1
#define ASDU_INVALID_IOA        -2
#define ASDU_INVALID_TYPE       -3
#define ASDU_TYPE_MISMATCH      -4

/*
*===============================================================================================================
*= = 结构体定义
*===============================================================================================================
*/

#pragma pack(push, 1)

/* U structure */
typedef struct {
    uint8_t     start;
    uint8_t     len;
    uint8_t     cb;
    uint8_t     cbu[3];
} apdu_u_t;

/* S structure */
typedef struct {
    uint8_t     start;
    uint8_t     len;
    uint8_t     cb;
    uint8_t     zero;
    uint16_t    ack;
} apdu_s_t;

/* I structure */
typedef struct {
    uint8_t     start;
    uint8_t     len;
    uint16_t    seq;
    uint16_t    ack;
    uint8_t     asdu[250];
} apdu_i_t;

/* Information object structure */
typedef struct iec_data {
    uint8_t     type;
    uint8_t     qds;
    uint8_t     flags;
    uint8_t     cause;
    uint32_t    ioa;
    struct timeval  tv;
    struct {
        uint16_t ca;
        uint32_t ioa;
    } link;
    iec_variant_t value;
    void* userdata;
} iec_data_t;

/* Sector structure */
typedef struct iec_sector {
    int         ca;             /* Common Address of ASDU */
    int         max_count;      /* Maximum values number */
    int         max_ioa;        /* Maximum IOA */
    int         count;          /* Current values count */
    struct timeval tv;          /* Last read data time */
    int         master;         /* Master ID or -1 */
    int         port;           /* Port */
    uint64_t    received;       /* Received values count */
    int         n_slaves;       /* Connected slaves count */
    int* slaves;         /* Connected slaves */
    int* indexes;        /* Index table */
    iec_data_t  data[0];        /* Data array */
} iec_sector_t;

/* Output queue item structure */
typedef struct queue_item {
    uint16_t    ca;             /* Common address of ASDU */
    iec_data_t  data;           /* Data structure */
} queue_item_t;

/* Output queue list */
typedef struct iec_queue {
    int             size;       /* Queue items count */
    queue_item_t    queue[0];   /* Queue items list */
} iec_queue_t;

typedef struct iec_socket iec_socket_t;

/* Connection structure */
typedef struct iec_connection {
    /* Used by IEC104 protocol */
    int             len;        /* Read bytes count */
    int             port;       /* IP port */
    char            addr[INET6_ADDRSTRLEN]; /* IP address */
    time_t          time_read;  /* Last read time */
    time_t          time_write; /* Last write time */
    iec_socket_t* parent;     /* Ptr to parent socket structure */
    uint16_t        ack;        /* IEC104: ACK */
    uint16_t        seq;        /* IEC104: SEQ */
    time_t          time_t1;    /* IEC104: last time of send unacknowledged message (for T1) */
    time_t          time_t3;    /* IEC104: last time of read or send TESTFR (for T3) */
    time_t          time_stop;  /* IEC104: last disconnect time */
    time_t          time_ic;    /* IEC104: last common interrogation time */
    int             n_in_i;     /* IEC104: received I-frames count since S-frame send (for W) */
    int             n_in;       /* IEC104: received I-frames count */
    int             e_in;       /* IEC104: protocol read errors */
    int             n_out_i;    /* IEC104: transmitted I-frames count since S-frame send (for K) */
    int             n_out;      /* IEC104: transmitted I-frames count */
    int             e_out;      /* IEC104: protocol write errors */
    /* Output buffer */
    union {
        apdu_i_t    apdu_i;
        apdu_s_t    apdu_s;
        apdu_u_t    apdu_u;
        char        buffer[256];
    } out;
    /* Input buffer */
    union {
        apdu_i_t    apdu_i;
        apdu_s_t    apdu_s;
        apdu_u_t    apdu_u;
        char        buffer[IN_BUF_SIZE];
    };
    connection_t    connection; /* TCP connection */
    int             started;    /* 1 if connected, otherwise 0 */
    iec_queue_t* queue;         /* Queue for transmission */
} iec_connection_t;

/* Socket structure */
typedef struct iec_socket {
    int         index;          /* Index of this socket structure in list */
    int         flags;          /* flags: client/server, master/slave, IPv4/6 */
    int         port;           /* IP port */
    char        addr[INET6_ADDRSTRLEN]; /* IP address */
    iec_settings_t  is;         /* IEC104 settings */
    union {
        /* if server */
        struct {
            server_t    server; /* Server socket */
            uint32_t    n_in;   /* Statistics (unused) */
            uint32_t    n_out;
            uint32_t    e_in;
            uint32_t    e_out;
            int         access_count;   /* Allowed clients count */
            void* access_list;    /* Allowed clients IP list */
        } server;
        /* if client */
        struct {
            client_t    client; /* Client socket */
            int         connecting; /* 1 if currently connecting, otherwise 0 */
            struct iec_connection   connection; /* Connection structure */
        } client;
    };
    int         sector_max;     /* Maximum sectors number */
    int         sector_count;   /* Current sectors count */
    iec_sector_t* sectors[0];  /* Sectors list */
} iec_socket_t;

/* Data unit identifier */
typedef struct {
    uint8_t     type;   /* type identification */
    uint8_t     vsq;    /* variable structure qualifier SQ|NUM(7) */
    uint8_t     cause;  /* cause of transmission PN|T|CAUSE(6) */
    uint8_t     oa;     /* originator address */
    uint16_t    ca;     /* common address of ASDU */
} data_unit_t;

typedef struct {
    uint32_t    ioa;
    union {
        asdu1   type1;
        asdu3   type3;
        asdu5   type5;
        asdu7   type7;
        asdu9   type9;
        asdu11  type11;
        asdu13  type13;
        asdu15  type15;
        asdu30  type30;
        asdu31  type31;
        asdu32  type32;
        asdu33  type33;
        asdu34  type34;
        asdu35  type35;
        asdu36  type36;
        asdu37  type37;
        asdu45  type45;
        asdu46  type46;
        asdu47  type47;
        asdu48  type48;
        asdu49  type49;
        asdu50  type50;
        asdu51  type51;
        asdu58  type58;
        asdu59  type59;
        asdu60  type60;
        asdu61  type61;
        asdu62  type62;
        asdu63  type63;
        asdu64  type64;
        asdu100 type100;
        asdu101 type101;
        asdu103 type103;
        asdu104 type104;
        asdu105 type105;
        asdu107 type107;
        uint8_t res[252];
    };
} iec_asdu_t;

/* Unpacked data unit */
typedef struct {
    uint8_t         type;
    uint8_t         cause;
    uint8_t         test;
    uint8_t         pn;
    uint16_t        ca;
    iec_asdu_t* obj;
} object_unit_t;

#pragma pack(pop)

#pragma pack(push, 1)
typedef struct {
    uint16_t ca;
    uint16_t type;
    uint32_t ioa;
    iec_variant_t value;
    struct timeval tv;
} store_data_t;
#pragma pack(pop)

#define STORE_DATA_NUM      1024
#define ASSERT(cond, code)  if (cond) {errno = code; goto error;}
#define ASSERT1(cond, code) if (cond) {errno = code; goto error1;}

#if defined(IEC_DEBUG_EVENT) || defined(IEC_DEBUG_FRAME)
static const char* ctx_names[] = {
    "?ctx0?",   "pec/cyc",  "back",     "spont",
    "init",     "req",      "act",      "actcon",
    "deact",    "deactcon", "actterm",  "retrem",
    "retloc",   "file",     "?ctx14?",  "?ctx15?",
    "?ctx16?",  "?ctx17?",  "?ctx18?",  "?ctx19?",
    "introgen", "intro1",   "intro2",   "intro3",
    "intro4",   "intro5",   "intro6",   "intro7",
    "intro8",   "intro9",   "intro10",  "intro11",
    "intro12",  "intro13",  "intro14",  "intro15",
    "intro16",  "reqcogen", "reqco1",   "reqco2",
    "reqco3",   "reqco4",   "?ctx42?",  "?ctx43?",
    "uktypeid", "ukctx",    "ukca",     "ukioa"
};
static const char* get_ctx_name(uint8_t cause) {
    static char buf[10];
    if (cause < sizeof(ctx_names) / sizeof(const char*)) {
        return ctx_names[cause];
    }
    else {
        sprintf(buf, "?ctx%d?", cause);
        return buf;
    }
}
#ifndef __ORCINUS_AS_SERVICE__
static void print_time() {
    char buf[32];
#ifdef IEC_LOG_TIME_MS
    struct timeval tv;
    gettimeofday(&tv, NULL);
    struct tm tm;
    localtime_r(&tv.tv_sec, &tm);
    strftime(buf, sizeof(buf), "%Y-%m-%d %H:%M:%S.", &tm);
    sprintf(&buf[20], "%03d ", (int)(tv.tv_usec / 1000));
    printf("%s", buf);
#else
    time_t t = time(NULL);
    struct tm tm;
    localtime_r(&t, &tm);
    strftime(buf, sizeof(buf), "%Y-%m-%d %H:%M:%S ", &tm);
    printf("%s", buf);
#endif
}
#endif
#endif

#ifdef IEC_DEBUG_EVENT
#ifdef __ORCINUS_AS_SERVICE__
#define LOG(level, fmt...) if (level <= IEC_LOG_LEVELS) syslog(LOG_PRIORITY, fmt);
#else
#define LOG(level, fmt...) if (level <= IEC_LOG_LEVELS) {print_time(); printf(fmt);}
#endif
#if IEC_LOG_LEVELS == IEC_LOG_ALL
static void _hexdump(void* buf, int len, int dir) {
    static char hex[] = "0123456789abcdef";
    char out[256 * 3];
    int i, j;
    char* p = (char*)buf;
    strcpy(out, dir ? "RX: " : "TX: ");
    for (i = 0, j = 4; i < len; i++) {
        out[j++] = hex[(*p >> 4) & 15];
        out[j++] = hex[*p++ & 15];
        out[j++] = ' ';
    }
    out[j++] = 0;
    printf("%s\n", out);
}
#define hexdump(buf, len, dir) _hexdump(buf, len, dir);
#else
#define hexdump(buf, len, dir)
#endif
#else
#define LOG(level, fmt, ...)
#define hexdump(buf, len, dir)
#endif

#ifdef IEC_DEBUG_FRAME
#ifdef __ORCINUS_AS_SERVICE__
#define LOG2(level, fmt...) if (level & (IEC_LOG_LEVELA)) syslog(LOG_PRIORITY, fmt);
#else
#define LOG2(level, fmt...) if (level & (IEC_LOG_LEVELA)) {print_time(); printf(fmt);}
#endif
#else
#define LOG2(level, fmt, ...)
#endif

/* ASDU sizes table */
static uint8_t asdu_sizes[256] = {
    255, sizeof(asdu1), 255, sizeof(asdu3), 255, sizeof(asdu5), 255,
        sizeof(asdu7),                                                          //   0 -   7
    255, sizeof(asdu9), 255, sizeof(asdu11), 255, sizeof(asdu13),
        255, sizeof(asdu15),                                                    //   8 -  15
    255, 255, 255, 255, 255, 255, 255, 255,                                     //  16 -  23
    255, 255, 255, 255, 255, 255, sizeof(asdu30), sizeof(asdu31),               //  24 -  31
    sizeof(asdu32), sizeof(asdu33), sizeof(asdu34), sizeof(asdu35),
        sizeof(asdu36), sizeof(asdu37), 255, 255,                               //  32 -  39
    255, 255, 255, 255, 255, sizeof(asdu45), sizeof(asdu46), sizeof(asdu47),    //  40 -  47
    sizeof(asdu48), sizeof(asdu49), sizeof(asdu50), sizeof(asdu51), 255, 255,
        255, 255,                                                               //  48 -  55
    255, 255, sizeof(asdu58), sizeof(asdu59), sizeof(asdu60),
        sizeof(asdu61), sizeof(asdu62), sizeof(asdu63),                         //  56 -  63
    sizeof(asdu64), 255, 255, 255, 255, 255, 255, 255,                          //  64 -  71
    255, 255, 255, 255, 255, 255, 255, 255,                                     //  72 -  79
    255, 255, 255, 255, 255, 255, 255, 255,                                     //  80 -  87
    255, 255, 255, 255, 255, 255, 255, 255,                                     //  88 -  95
    255, 255, 255, 255, sizeof(asdu100), sizeof(asdu101), 255, sizeof(asdu103), //  96 - 103
    sizeof(asdu104), 255, 255, 255, 255, 255, 255, 255,                         // 104 - 111
    255, 255, 255, 255, 255, 255, 255, 255,                                     // 112 - 119
    255, 255, 255, 255, 255, 255, 255, 255,                                     // 120 - 127
    255, 255, 255, 255, 255, 255, 255, 255,                                     // 128 - 135
    255, 255, 255, 255, 255, 255, 255, 255,                                     // 136 - 143
    255, 255, 255, 255, 255, 255, 255, 255,                                     // 144 - 151
    255, 255, 255, 255, 255, 255, 255, 255,                                     // ...
    255, 255, 255, 255, 255, 255, 255, 255,                                     //
    255, 255, 255, 255, 255, 255, 255, 255,                                     //
    255, 255, 255, 255, 255, 255, 255, 255,                                     //
    255, 255, 255, 255, 255, 255, 255, 255,                                     //
    255, 255, 255, 255, 255, 255, 255, 255,                                     //
    255, 255, 255, 255, 255, 255, 255, 255,                                     //
    255, 255, 255, 255, 255, 255, 255, 255,                                     //
    255, 255, 255, 255, 255, 255, 255, 255,                                     //
    255, 255, 255, 255, 255, 255, 255, 255,                                     //
    255, 255, 255, 255, 255, 255, 255, 255,                                     //
    255, 255, 255, 255, 255, 255, 255, 255,                                     //
    255, 255, 255, 255, 255, 255, 255, 255                                      //
};

/* ASDU value types table */
static uint8_t asdu_value_types[256] = {
    0, IECT_SINGLE, 0, IECT_DOUBLE, 0, IECT_STEP, 0, IECT_BITSTRING,            //   0 -   7
    0, IECT_NORMALIZED, 0, IECT_SCALED, 0, IECT_FLOAT, 0, IECT_INTEGRATED,      //   8 -  15
    0, 0, 0, 0, 0, 0, 0, 0,                                                     //  16 -  23
    0, 0, 0, 0, 0, 0, IECT_SINGLE, IECT_DOUBLE,                                 //  24 -  31
    IECT_STEP, IECT_BITSTRING, IECT_NORMALIZED, IECT_SCALED,
        IECT_FLOAT, IECT_INTEGRATED, 0, 0,                                      //  32 -  39
    0, 0, 0, 0, 0, IECC_SINGLE, IECC_DOUBLE, IECC_STEP,                         //  40 -  47
    IECC_NORMALIZED, IECC_SCALED, IECC_FLOAT, IECC_BITSTRING, 0, 0, 0, 0,       //  48 -  55
    0, 0, IECC_SINGLE, IECC_DOUBLE, IECC_STEP, IECC_NORMALIZED, IECC_SCALED,
        IECC_FLOAT,                                                             //  56 -  63
    IECC_BITSTRING, 0, 0, 0, 0, 0, 0, 0,                                        //  64 -  71
    0, 0, 0, 0, 0, 0, 0, 0,                                                     //  72 -  79
    0, 0, 0, 0, 0, 0, 0, 0,                                                     //  80 -  87
    0, 0, 0, 0, 0, 0, 0, 0,                                                     //  88 -  95
    0, 0, 0, 0, 0, 0, 0, 0,                                                     //  96 - 103
    0, 0, 0, 0, 0, 0, 0, 0,                                                     // 104 - 111
    0, 0, 0, 0, 0, 0, 0, 0,                                                     // 112 - 119
    0, 0, 0, 0, 0, 0, 0, 0,                                                     // 120 - 127
    0, 0, 0, 0, 0, 0, 0, 0,                                                     // 128 - 135
    0, 0, 0, 0, 0, 0, 0, 0,                                                     // 136 - 143
    0, 0, 0, 0, 0, 0, 0, 0,                                                     // 144 - 151
    0, 0, 0, 0, 0, 0, 0, 0,                                                     // ...
    0, 0, 0, 0, 0, 0, 0, 0,                                                     //
    0, 0, 0, 0, 0, 0, 0, 0,                                                     //
    0, 0, 0, 0, 0, 0, 0, 0,                                                     //
    0, 0, 0, 0, 0, 0, 0, 0,                                                     //
    0, 0, 0, 0, 0, 0, 0, 0,                                                     //
    0, 0, 0, 0, 0, 0, 0, 0,                                                     //
    0, 0, 0, 0, 0, 0, 0, 0,                                                     //
    0, 0, 0, 0, 0, 0, 0, 0,                                                     //
    0, 0, 0, 0, 0, 0, 0, 0,                                                     //
    0, 0, 0, 0, 0, 0, 0, 0,                                                     //
    0, 0, 0, 0, 0, 0, 0, 0,                                                     //
    0, 0, 0, 0, 0, 0, 0, 0                                                      //
};

/* Variable type sizes table */
static uint8_t asdu_type_sizes[IECT_MAX + 1] = {
    //  single     double     step       bitstring  normalized scaled     float      integrated
    0,  1,         1,         1,         4,         2,         2,         4,         4
};

/* ASDU value types for var types */
static uint8_t asdu_types[2][IECT_MAX + 1] = {
    //  single     double     step       bitstring  normalized scaled     float      integrated   
    {0, M_SP_NA_1, M_DP_NA_1, M_ST_NA_1, M_BO_NA_1, M_ME_NA_1, M_ME_NB_1, M_ME_NC_1, M_IT_NA_1},
    {0, M_SP_TB_1, M_DP_TB_1, M_ST_TB_1, M_BO_TB_1, M_ME_TD_1, M_ME_TE_1, M_ME_TF_1, M_IT_TB_1}
};

/* ASDU commands */
static uint8_t asdu_types2[2][IECT_MAX] = {
    //  single     double     step       bitstring  normalized scaled     float      
    {0, C_SC_NA_1, C_DC_NA_1, C_RC_NA_1, C_BO_NA_1, C_SE_NA_1, C_SE_NB_1, C_SE_NC_1},
    {0, C_SC_TA_1, C_DC_TA_1, C_RC_TA_1, C_BO_TA_1, C_SE_TA_1, C_SE_TB_1, C_SE_TC_1}
};

#define IECF_RUNNING    0x01
#define IECF_STOPPING   0x02
#define IECF_SYNC_EXT   0x04

/* IEC104 control block */
static struct {
    uint16_t                sector_max;             /* Maximum sectors number */
    uint16_t                socket_max;             /* Maximum sockets number */
    uint16_t                socket_count;           /* Current sockets number */
    iec_sector_t**          sectors;                /* Sector structures list */
    iec_socket_t**          sockets;                /* Socket structures list */
    syncobj_t               iec_sync;               /* Common synchronization object */
    synctimer_t             iec_timer;              /* Timer */
    iec_settings_t          is;                     /* General IEC104 settings structure */
    iec_object_listener_t   iec_object_listener;    /* Information object listener function */
    iec_status_listener_t   iec_status_listener;    /* Status listener function */
    iec_event_listener_t    iec_event_listener;     /* Event listener function */
    iec_command_func_t      iec_cmd_func;           /* Command function */
    iec_timer_func_t        iec_timer_func;         /* External timer function */
    int                     flags;                  /* Internal flags */
    int                     running;                /* =1 if IEC is running */
    int                     stopping;               /* =1 if IEC is stopping */
    char                    event[256];             /* Event message */
    void*                   buf;                    /* Mixed IEC data buffers */
    char                    errormsg[256];          /* Error message */
    int                     c_ioa_len;              /* Current IOA length */
    int                     c_ca_len;               /* Current CA length */
    int                     c_oa;                   /* Current OA */
} icb;

/*
*===============================================================================================================
*= = 全局变量
*===============================================================================================================
*/

static const char ERR_SOCKET_NEW[] = "Error #%d create socket %s:%d, flags=%02x: %s\n";
static const char ERR_SERVER_NEW[] = "Error #%d create server at port %d: %s\n";
static const char ERR_CLIENT_NEW[] = "Error #%d create client %s:%d: %s\n";
static const char ERR_TIMER_START[] = "Timer start error %d: %s\n";
static const char ERR_IEC_START_EMPTY[] = "Start error: no sockets defined\n";
static const char ERR_IEC_START_NOSEC[] = "Start error: no sectors assigned to socket #%d\n";
static const char ERR_INIT[] = "Init error %d: %s\n";
static const char ERR_INIT_DATA[] = "Init error %d: invalid value in file %s\n";
static const char ERR_SERVER_START[] = "Error #%d start server #%d at port %d: %s\n";
static const char ERR_SECTOR_NEW[] = "Error #%d create sector %d: %s\n";
static const char ERR_VALUE_ADD[] = "Error adding value %s type to {%d, %d}: %s\n";
static const char ERR_VALUES_ADD[] = "Error adding %d values to sector %d: %s\n";
static const char ERR_CMD_ADD[] = "Error adding command %s type to {%d, %d} linked to {%d, %d}: %s\n";
static const char ERR_VALUE_DUP[] = "Error duplicate value {%d, %d} to {%d, %d}: %s\n";
static const char ERR_SOCKET_ADD[] = "Error add sector %d to socket #%d: %s\n";
static const char ERR_IN_FILE[] = "Error in file %s at line %d: %s\n";
static const char ERR_IN_FILE1[] = "Error in file %s at line %d: %s (%d)\n";
static const char ERR_LOAD_DATA[] = "Error loading data: %s\n";
static const char ERR_LOAD_FILE[] = "Error load file %s: %s\n";
static const char ERR_ALLOW_IP_LIST[] = "Error allow IP %s to socket at port %d: %s\n";
static const char ERR_IN_DATA_CFG[] = "Error load data[%d] at line %d: %s\n";
static const char ERR_IN_SOCKETS_CFG[] = "Error load sockets at line %d: %s\n";
static const char ERR_IN_SOCKETS_CFG1[] = "Error load sockets at line %d: %s (%d)\n";

static const char EVT_CONNECTED[] = "%s connected\n";
static const char EVT_DISCONNECTED[] = "%s disconnected\n";
static const char EVT_ERROR[] = "%s error #%d (%s)\n";
static const char EVT_READ[] = "%s read %d bytes\n";
static const char EVT_LISTEN[] = "%s listen\n";
static const char EVT_CLOSED[] = "%s closed\n";
static const char EVT_ACCEPTED[] = "%s accepted\n";
static const char EVT_STARTING[] = "IEC starting\n";
static const char EVT_STOPPED[] = "IEC stopped\n";
static const char EVT_ACCESS_DENIED[] = "Access denied for %s:%d\n";

#ifdef IEC_DEBUG_EVENT

static const char ERR_COMMAND_TYPE[] = "Command type mismatch: type=%d, ca=%d, ioa=%d at %s\n";
static const char ERR_CLIENT_OPEN[] = "Error #%d open client #%d: %s\n";
static const char ERR_UNKNOWN_CAUSE[] = "Unknown cause: type=%d, ca=%d, ioa=%d, cause=%d at %s\n";
static const char ERR_START_BYTE[] = "Start byte missing at %s\n";
static const char ERR_SEQUENCE_VIOLATION[] = "%c-frame sequence violation at %s\n";
static const char ERR_INVALID_CMD[] = "RX: Invalid command: %d at %s\n";
static const char ERR_PARSING_ASDU_TYPE[] = "Error parsing ASDU: unknown type=%d, ca=%d, count=%d at %s\n";
static const char ERR_PARSING_ASDU[] = "Error parsing ASDU at %s\n";
static const char ERR_ASDU_INVALID_IOA[] = "Invalid ASDU IOA: cause=%s, type=%d, ca=%d, ioa=%u at %s\n";
static const char ERR_ASDU_INVALID_TYPE[] = "Invalid ASDU type: cause=%s, type=%d, ca=%d, ioa=%u at %s\n";
static const char ERR_ASDU_TYPE_MISMATCH[] = "Invalid ASDU type mismatch: cause=%s, type=%d, ca=%d, ioa=%u at %s\n";
static const char ERR_ASDU_INVALID_CA[] = "Invalid ASDU CA: cause=%s, type=%d, ca=%d, count=%d at %s\n";
static const char ERR_ASDU_INVALID_CMD[] = "Invalid ASDU cmd: cause=%s, type=%d, ca=%d, count=%d at %s\n";
static const char IO_ASDU[] = "%cX ASDU: cause=%s, type=%d, ca=%d, count=%d at %s\n";

#endif

#if defined(IEC_DEBUG_EVENT) || defined(IEC_DEBUG_FRAME)
static const char IO_TYPE_U[] = "%cX: Type U [%s] at %s\n";
static const char IO_TYPE_S[] = "%cX: Type S [ACK=%d] at %s\n";
static const char IO_TYPE_I[] = "%cX: Type I [LEN=%d, ACK=%d, SEQ=%d] at %s\n";

static const char* STARTDT_ACT_N = "STARTDT_ACT";
static const char* STARTDT_CON_N = "STARTDT_CON";
static const char* TESTFR_ACT_N = "TESTFR_ACT";
static const char* TESTFR_CON_N = "TESTFR_CON";
static const char* STOPDT_ACT_N = "STOPDT_ACT";
static const char* STOPDT_CON_N = "STOPDT_CON";

static const char* get_u_name(int flag) {
    static char buf[16];
    switch (flag) {
    case STARTDT_ACT: return STARTDT_ACT_N;
    case STARTDT_CON: return STARTDT_CON_N;
    case TESTFR_ACT: return TESTFR_ACT_N;
    case TESTFR_CON: return TESTFR_CON_N;
    case STOPDT_ACT: return STOPDT_ACT_N;
    case STOPDT_CON: return STOPDT_CON_N;
    default:
        sprintf(buf, "UNKNOWN U %d", flag);
        return buf;
    }
}
#endif

static const char UNKNOWN[] = "UNKNOWN";
static const char _LOGDIR[] = "log";
const char* default_log_dir = _LOGDIR;

static int nsectors = 0, nsockets = 0, maxsector = 0;
static int status_sector = 0;

static sector_t* sectors = NULL;
static socket_t* sockets = NULL;

static char iec104_types[IECT_MAX] = {
    IECT_SINGLE, IECT_DOUBLE, IECT_STEP, IECT_BITSTRING, IECT_NORMALIZED, IECT_SCALED, IECT_FLOAT, IECT_INTEGRATED
};

/*
*===============================================================================================================
*= = 函数定义
*===============================================================================================================
*/

static int load_iec_options(char** bufptr, iec_settings_t* is, int* flags);
static int iec_command(iec_socket_t* s, iec_sector_t* sector, object_unit_t* info, iec_connection_t* c);
static int iec_command0(iec_socket_t* s, object_unit_t* info, iec_connection_t* c);
static int iec_slave_enqueue_sector(iec_connection_t* c, iec_sector_t* sector);
static int iec_sector_update_value(iec_sector_t* sector, object_unit_t* info, iec_data_t** dataptr);
static int slave_read(iec_connection_t* c);
static int master_read(iec_connection_t* c);
static int iec_sector_max_num(uint8_t type, uint8_t sq);
static int iec_send_u(iec_connection_t* c, uint8_t cmd);

/*
*---------------------------------------------------------------------------------------------------------------
*= = Cross Platform - Windows
*---------------------------------------------------------------------------------------------------------------
*/

#ifdef WIN32
int gettimeofday(struct timeval* tp, void* tzp)
{
    time_t clock;
    struct tm tm;
    SYSTEMTIME wtm;

    GetLocalTime(&wtm);
    tm.tm_year = wtm.wYear - 1900;
    tm.tm_mon = wtm.wMonth - 1;
    tm.tm_mday = wtm.wDay;
    tm.tm_hour = wtm.wHour;
    tm.tm_min = wtm.wMinute;
    tm.tm_sec = wtm.wSecond;
    tm.tm_isdst = -1;

    clock = mktime(&tm);
    tp->tv_sec = clock;
    tp->tv_usec = wtm.wMilliseconds * 1000;
    return (0);
}

struct tm* localtime_r(const time_t* timep, struct tm* result)
{
    struct tm* tmp = localtime(timep);

    if (tmp != NULL)
    {
        memcpy(result, tmp, sizeof(struct tm));
    }

    return tmp;
}

unsigned int sleep(unsigned int seconds)
{
    Sleep(seconds * 1000);

    return 0;
}
#endif

/*
*---------------------------------------------------------------------------------------------------------------
*= = INDEX FUNCTIONS
*---------------------------------------------------------------------------------------------------------------
*/

static char* socket_info(iec_socket_t* s)
{
    static char buf[256] = { 0 };
    if (s->flags & _IECF_S_CLIENT)
    {
        sprintf(buf, "A/%c %s:%d", IEC_GET_MS(s), s->addr, s->port);
    }
    else {
        sprintf(buf, "P/%c ipv%c:%d", IEC_GET_MS(s), IEC_GET_IPV(s), s->port);
    }

    return buf;
}

static char* connection_info(iec_connection_t* c)
{
    static char buf[256] = {0};

    if (c == NULL)
    {
        strcpy(buf, UNKNOWN);
    }
    else
    {
        connection_t con = c->connection;
        iec_socket_t* s = c->parent;
        if (con == NULL)
        {
            return socket_info(s);
        }
        else
        {
            if (s->flags & _IECF_S_CLIENT)
            {
                return socket_info(s);
            }
            else
            {
                sprintf(buf, "P/%c %s:%d", IEC_GET_MS(s), c->addr, s->port);
            }
        }
    }
    return buf;
}

/*
 * Return index of selected IOA if one is exists 
 * otherwise return -1 and set value pointed by new_pos to index of new value to insert
 */
static int iec_sector_search_ioa(iec_sector_t* sector, uint32_t ioa, int* new_pos)
{
    int first = 0;
    int last = sector->count;
    iec_data_t* data = sector->data;
    if (last == 0 || data[0].ioa > ioa)
    {
        *new_pos = 0;
        return -1;
    }
    else if (data[last - 1].ioa < ioa)
    {
        *new_pos = last;
        return -1;
    }

    while (first < last)
    {
        int mid = (first + last) / 2;
        if (ioa == data[mid].ioa)
            return mid;
        else if (ioa <= data[mid].ioa)
            last = mid;
        else
            first = mid + 1;
    }

    if (data[last].ioa == ioa)
        return last;
    *new_pos = last;
    return -1;
}

/* Return data structure ptr of sector by IOA if one is exists, otherwise NULL */
static iec_data_t* iec_sector_data(iec_sector_t* sector, uint32_t ioa)
{
    int index, new_pos;
    //if (s->count == 0) return NULL;
    if (ioa == 0) return NULL;
    index = sector->indexes != NULL ? sector->indexes[ioa - 1] : iec_sector_search_ioa(sector, ioa, &new_pos);
    return index < 0 ? NULL : &sector->data[index];
}

/*
 * Return ptr to data structure by common address of ASDU and IOA
 */
static iec_data_t* iec_object_get_data(const int ca, uint32_t ioa)
{
    iec_sector_t* s;
    if ((s = IEC_SECTOR(ca)) == NULL || ioa - 1 >= s->max_ioa)
        return NULL;
    return iec_sector_data(s, ioa);
}

/* Add new data structure to sector by IOA if one is not exists and return ptr to it, otherwise return NULL */
static iec_data_t* iec_sector_data_new(iec_sector_t* sector, uint32_t ioa)
{
    int index, new_pos;
    if (sector->indexes != NULL)
    {
        index = sector->indexes[ioa - 1];
        if (index >= 0) {
            errno = EI_IOAEXIST;
            return NULL;
        }
        sector->indexes[ioa - 1] = index = sector->count++;
        return &sector->data[index];
    }
    if (sector->count == 0) return &sector->data[sector->count++];
    index = iec_sector_search_ioa(sector, ioa, &new_pos);
    if (index >= 0)
    {
        errno = EI_IOAEXIST;
        return NULL;
    }
    if (new_pos < sector->count)
        memmove(&sector->data[new_pos + 1], &sector->data[new_pos], (sector->count - new_pos) * sizeof(iec_data_t));
    sector->count++;
    return &sector->data[new_pos];
}

static void send_queue(iec_socket_t* s, iec_connection_t* c);
static iec_connection_t* iec_server_connection(iec_socket_t* s, int index);

/*
 * Add data to queue
 */
static void iec_enqueue_item(iec_connection_t* c, queue_item_t* item)
{
    iec_queue_t* q = c->queue;
    if (q->size == QUEUE_SIZE)
    {
        send_queue(c->parent, c);
    }
    q->queue[q->size++] = *item;
}

/*
 * Puts data to output queue
 * Params:
 * sector:      Ptr to sector structure
 * ioa:         Information Object Address
 * tv:          Ptr to <struct timeval>; if NULL use current time
 */
static void iec_sector_enqueue_data(iec_sector_t* sector, uint32_t ioa, struct timeval* tv, uint8_t cause)
{
    int i, j, count;
    iec_socket_t* s;
    queue_item_t q;
    struct timeval t;
    if (sector->n_slaves == 0) return;
    iec_data_t* data = iec_sector_data(sector, ioa);
    if (data == NULL) return;
    if (tv == NULL) gettimeofday(&t, NULL);
    if (data->type == 0 || (data->flags & IECF_O_NO_SPONT) != 0 || (data->type & 0x80) != 0) return;
    q.ca = sector->ca;
    q.data = *data;
    q.data.cause = cause;
    q.data.tv = tv ? *tv : t;
    for (i = 0; i < sector->n_slaves; i++)
    {
        s = icb.sockets[sector->slaves[i]];
        if (s->flags & _IECF_S_CLIENT)
        {
            iec_enqueue_item(&s->client.connection, &q);
        }
        else
        {
            for (j = 0, count = server_get_count(s->server.server); j < count; j++)
            {
                iec_connection_t* c = iec_server_connection(s, j);
                iec_enqueue_item(c, &q);
            }
        }
    }
}

/*
 * Internal realization of iec_object_set_value(); read more below
 */
static int iec_object_set_value_int(iec_sector_t* sector, iec_data_t* data, void* value, uint16_t* qds, struct timeval* tv)
{
    int size, changed = 0;
    if (tv != NULL && (data->tv.tv_sec != tv->tv_sec || data->tv.tv_usec != tv->tv_usec))
    {
        changed = 1;
        data->tv = *tv;
    }
    size = asdu_type_sizes[data->type & 0x7f];
    if (value != NULL) {
        switch (size) {
        case 1:
            if (data->value.uint8 != *(uint8_t*)value)
            {
                changed = 1;
                data->value.uint8 = *(uint8_t*)value;
            }
            break;
        case 2:
            if (data->value.uint16 != *(uint16_t*)value)
            {
                changed = 1;
                data->value.uint16 = *(uint16_t*)value;
            }
            break;
        case 4:
            if (data->value.uint32 != *(uint32_t*)value)
            {
                changed = 1;
                data->value.uint32 = *(uint32_t*)value;
            }
            break;
        default:                // ???
            errno = EINVAL;
            return -1;
        }
        if ((data->type & 0x80) != 0 && icb.iec_cmd_func != NULL)
        {
            // Set data->link to value
            icb.iec_cmd_func(data->link.ca, data->link.ioa, &data->value);
        }
    }

    if (qds != NULL)
    {
        int old_qds = data->qds;
        if (value != NULL) data->qds &= ~(IECF_NT | IECF_IV);
        int set = *qds & 0xff00;
        *qds &= 0xff;
        switch (set) {
        case IECF_QDS_COPY: data->qds = *qds; break;
        case IECF_QDS_SET: data->qds |= *qds; break;
        case IECF_QDS_RESET: data->qds &= ~(*qds); break;
        default:
            errno = EINVAL;
            return -1;
        }
        if (data->qds != old_qds) changed = 1;
    }
    else {
        if ((data->qds & (IECF_NT | IECF_IV)) != 0 && (data->qds & (IECF_SB | IECF_BL)) == 0) {
            data->qds &= ~(IECF_NT | IECF_IV);
            changed = 1;
        }
    }
    if ((data->flags & IECF_O_NO_SPONT) == 0)
        iec_sector_enqueue_data(sector, data->ioa, tv, changed ? IEC_CT_SPONT : IEC_CT_BGSCAN);
    if ((data->type & 0x80) == 0 && data->link.ca != 0) {
        uint16_t qds = data->qds;
        iec_object_set_value(data->link.ca, data->link.ioa, &data->value, &qds, &data->tv);
    }
    return 0;
}

static void update_status_sector(iec_socket_t* s, int status)
{
    if (status_sector == 0) return;
    iec_sector_t* s_sector = IEC_SECTOR(status_sector);
    if (s_sector == NULL)
    {
        return;
    }

    int i;
    struct timeval tv;
    gettimeofday(&tv, NULL);
    for (i = 0; i < s->sector_count; i++)
    {
        int ca = s->sectors[i]->ca;
        iec_data_t* data = iec_object_get_data(status_sector, ca);
        int8_t value = data->value.int8;
        if (status)
        {
            value &= ~IECF_SECTOR_ERROR;
        }
        else
        {
            value |= IECF_SECTOR_ERROR;
        }
        iec_object_set_value_int(s_sector, data, &value, NULL, &tv);
        //icb.iec_status_listener(s->sectors[i]->ca, 1);
    }
}

/*
 * Client socket listener function
 */
static void iec_client_listener(event_t* e)
{
    int i, j, len;
    //    uint16_t qds;
    iec_socket_t* s = (iec_socket_t*)client_get_data(e->client);
    iec_connection_t* c = &s->client.connection;
    iec_sector_t* sector;
    time_t t = time(NULL);
    switch (e->type) {
    case SE_CONNECT:
        c->time_stop = c->len = c->ack = c->seq = 0;
        s->client.connecting = 0;
        c->time_read = c->time_write = c->time_t3 = t;
        c->connection = e->connection;
        EVENT(EVT_CONNECTED, connection_info(c));
        if (s->flags & _IECF_S_MASTER) {
            iec_send_u(c, STARTDT_ACT);
            if (icb.iec_status_listener != NULL) {
                for (i = 0; i < s->sector_count; i++)
                    icb.iec_status_listener(s->sectors[i]->ca, 1);
            }
            update_status_sector(s, 1);
        }
        break;
    case SE_DISCONNECT:
        c->time_stop = t;
        s->client.connecting = 0;
        EVENT(EVT_DISCONNECTED, connection_info(c));
        if (s->flags & _IECF_S_MASTER) {
            if (icb.iec_status_listener != NULL)
                for (i = 0; i < s->sector_count; i++)
                    icb.iec_status_listener(s->sectors[i]->ca, 0);
            if ((icb.flags & IECF_STOPPING) == 0) {
                update_status_sector(s, 0);
                for (i = 0; i < s->sector_count; i++) {
                    sector = s->sectors[i];
                    for (j = 0; j < sector->count; j++) {
                        if ((sector->data[j].qds & (IECF_SB | IECF_BL)) == 0)
                            sector->data[j].qds |= IECF_IV;
                    }
                }
            }
        }
        break;
    case SE_ERROR:
        c->time_stop = t;
        EVENT(EVT_ERROR, connection_info(c), e->error, strerror(e->error));
        s->client.connecting = 0;
        break;
    case SE_READ:
        c->time_read = t;
        len = 0;
        LOG(IEC_LOG_ALL, EVT_READ, connection_info(c), e->io.len);
        while (e->io.len > 0) {
            i = c->len + e->io.len > IN_BUF_SIZE
                ? IN_BUF_SIZE - c->len
                : e->io.len;
            memmove(c->buffer + c->len, e->io.buffer + len, i);
            len += i;
            c->len += i;
            e->io.len -= i;
            if (((s->flags & _IECF_S_MASTER) != 0 ? master_read(c) : slave_read(c)) < 0) {
                client_close(s);
                return;
            }
        }
        break;
    }
}

/*
 * Check if connection is in allowed list
 */
static int is_connection_allowed(iec_socket_t* s, connection_t c)
{
    int i, count;

    if ((count = s->server.access_count) == 0) return 1;
    if (s->flags && _IECF_S_IPV6)
    {
        struct in6_addr* in6 = (struct in6_addr*)(s->server.access_list);
        struct sockaddr_in6* sin6 = connection_get_peer6(c);
        if (sin6 == NULL) return 0;
        for (i = 0; i < count; i++)
        {
            if (memcmp(&sin6->sin6_addr, &in6[i], sizeof(struct in6_addr)) == 0)
            {
                return 1;
            }
        }
    }
    else
    {
        struct in_addr* in4 = (struct in_addr*)(s->server.access_list);
        struct sockaddr_in* sin4 = connection_get_peer(c);
        if (sin4 == NULL) return 0;
        for (i = 0; i < count; i++)
            if (memcmp(&sin4->sin_addr, &in4[i], sizeof(struct in_addr)) == 0) return 1;
    }
    return 0;
}

/*
 * Server socket listener function
 */
static void iec_server_listener(event_t* e)
{
    int i, j, len, size, qsize;
    iec_socket_t* s = (iec_socket_t*)server_get_data(e->server);
    iec_connection_t* c;
    iec_sector_t* sector;
    uint16_t qds;
    time_t t = time(NULL);
    void* ptr;
    switch (e->type) {
    case SE_LISTEN:
        EVENT(EVT_LISTEN, socket_info(s));
        break;
    case SE_CLOSE:
        EVENT(EVT_CLOSED, socket_info(s));
        break;
    case SE_ACCEPT:
        if (!is_connection_allowed(s, e->connection))
        {
            EVENT(EVT_ACCESS_DENIED, connection_get_address(e->connection), connection_get_lport(e->connection));
            e->error = EACCES;
            //connection_close(e->connection);
            break;
        }
        size = sizeof(struct iec_connection);
        qsize = (s->flags & _IECF_S_MASTER) ? 0 : sizeof(struct iec_queue) + QUEUE_SIZE * sizeof(queue_item_t);
        c = (iec_connection_t*)calloc(1, size + qsize);
        if (c == NULL) {
            e->error = errno;
            return;
        }
        if (qsize) c->queue = (iec_queue_t*)((intptr_t)c + size);
        c->parent = s;
        c->port = connection_get_rport(e->connection);
        strncpy(c->addr, connection_get_address(e->connection), sizeof(c->addr));
        c->connection = e->connection;
        c->time_read = c->time_write = c->time_t3 = t;
        connection_set_data(e->connection, c);
        EVENT(EVT_ACCEPTED, connection_info(c));
        break;
    case SE_CONNECT:
        c = (iec_connection_t*)connection_get_data(e->connection);
        c->time_stop = c->len = c->ack = c->seq = 0;
        c->time_read = c->time_write = c->time_t3 = t;
        EVENT(EVT_CONNECTED, connection_info(c));
        if (s->flags & _IECF_S_MASTER)
        {
            update_status_sector(s, 1);
            iec_send_u(c, STARTDT_ACT);
            if (icb.iec_status_listener != NULL)
                for (i = 0; i < s->sector_count; i++)
                    icb.iec_status_listener(s->sectors[i]->ca, 1);
        }
        break;
    case SE_DISCONNECT:
        c = (iec_connection_t*)connection_get_data(e->connection);
        if (c == NULL) {
            EVENT(EVT_DISCONNECTED, connection_get_address(e->connection));
            break;
        }
        c->time_stop = t;
        EVENT(EVT_DISCONNECTED, connection_info(c));
        if (s->flags & _IECF_S_MASTER) {
            if (icb.iec_status_listener != NULL)
                for (i = 0; i < s->sector_count; i++)
                    icb.iec_status_listener(s->sectors[i]->ca, 0);
            if ((icb.flags & IECF_STOPPING) == 0) {
                update_status_sector(s, 0);
                qds = IECF_IV | IECF_QDS_SET;
                struct timeval tv;
                gettimeofday(&tv, NULL);
                for (i = 0; i < s->sector_count; i++) {
                    sector = s->sectors[i];
                    for (j = 0; j < sector->count; j++)
                        if ((sector->data[j].qds & (IECF_SB | IECF_BL)) == 0)
                            iec_object_set_value_int(sector, &sector->data[j], NULL, &qds, &tv);
                }
            }
        }
        ptr = connection_get_data(e->connection);
        if (ptr != NULL) {
            connection_set_data(e->connection, NULL);
            free(ptr);
        }
        break;
    case SE_ERROR:
        if (e->connection == NULL) {
            LOG(IEC_LOG_ERROR, EVT_ERROR, socket_info(s), e->error, strerror(e->error));
        }
        else {
            c = (iec_connection_t*)connection_get_data(e->connection);
            LOG(IEC_LOG_ERROR, EVT_ERROR, c == NULL ? connection_address(e->connection) : connection_info(c), e->error, strerror(e->error));
        }
        break;
    case SE_READ:
        c = (iec_connection_t*)connection_get_data(e->connection);
        if (c == NULL) return;
        c->time_read = t;
        len = 0;
        LOG(IEC_LOG_ALL, EVT_READ, connection_info(c), e->io.len);
        while (e->io.len > 0) {
            i = c->len + e->io.len > IN_BUF_SIZE
                ? IN_BUF_SIZE - c->len
                : e->io.len;
            memmove(c->buffer + c->len, e->io.buffer + len, i);
            len += i;
            c->len += i;
            e->io.len -= i;
            if (((s->flags & _IECF_S_MASTER) != 0 ? master_read(c) : slave_read(c)) < 0) {
                connection_close(e->connection);
                return;
            }
        }
        break;
    }
}

/*
 * Send len bytes from buffer to selected connection
 */
static int iec_send(iec_connection_t* c, void* buffer, int len)
{
    int nbytes;
    c->time_write = time(NULL);
    //    m->n_i = 0;
    return connection_write(c->connection, buffer, len, &nbytes) == 0 && nbytes == len ? 0 : -1;
}

/*
 * Send U frame width command <cmd>
 */
static int iec_send_u(iec_connection_t* c, uint8_t cmd)
{
    c->out.apdu_u.start = APDU_START;
    c->out.apdu_u.len = 4;
    c->out.apdu_u.cb = cmd | 3;
    c->out.apdu_u.cbu[0] = 0;
    c->out.apdu_u.cbu[1] = 0;
    c->out.apdu_u.cbu[2] = 0;
    hexdump(&c->out.apdu_u, 6, 0);
    LOG(IEC_LOG_ALL, IO_TYPE_U, 'T', get_u_name(cmd), connection_info(c));
    LOG2(IEC_LOG_U, IO_TYPE_U, 'T', get_u_name(cmd), connection_info(c));
    return iec_send(c, &c->out.apdu_u, 6);
}

/*
 * Send S frame
 */
static int iec_send_s(iec_connection_t* c)
{
    c->out.apdu_s.start = APDU_START;
    c->out.apdu_s.len = 4;
    c->out.apdu_s.cb = 1;
    c->out.apdu_s.zero = 0;
    c->out.apdu_s.ack = c->ack << 1;
    hexdump(&c->out.apdu_s, 6, 0);
    LOG(IEC_LOG_ALL, IO_TYPE_S, 'T', c->ack, connection_info(c));
    LOG2(IEC_LOG_S, IO_TYPE_S, 'T', c->ack, connection_info(c));
    return iec_send(c, &c->out.apdu_s, 6);
}

/*
 * Send I frame
 */
static int iec_send_i(iec_connection_t* c, void* asdu, uint8_t len)
{
    if (len > 249) return -1;
    c->out.apdu_i.start = APDU_START;
    c->out.apdu_i.len = len + 4;
    c->out.apdu_i.seq = c->seq << 1;
    c->out.apdu_i.ack = c->ack << 1;
    c->seq++;
    c->n_out++;
    c->n_out_i++;
    if (len > 0) memmove(&c->out.apdu_i.asdu, asdu, len);
    hexdump(&c->out.apdu_i, len + 6, 0);
    LOG(IEC_LOG_ALL, IO_TYPE_I, 'T', len, c->ack, c->seq, connection_info(c));
    LOG2(IEC_LOG_I, IO_TYPE_I, 'T', len, c->ack, c->seq, connection_info(c));
    return iec_send(c, &c->out.apdu_i, len + 6);
}

/*
*---------------------------------------------------------------------------------------------------------------
*= = TIME CONVERSION FUNCTIONS
*---------------------------------------------------------------------------------------------------------------
*/

static void tv_to_cp56time2a(cp56time2a* tm, struct timeval* tv);
static void time_t_to_cp56time2a(cp56time2a* tm, time_t* t);

/*
 * Convert POSIX <struct timeval> to IEC104 time <cp56time2a>
 */
static void tv_to_cp56time2a(cp56time2a* tm, struct timeval* tv)
{
    time_t_to_cp56time2a(tm, &tv->tv_sec);
    tm->msec += (tv->tv_usec + 500) / 1000;
}

/*
 * Convert POSIX <time_t> to IEC104 time <cp56time2a>
 */
static void time_t_to_cp56time2a(cp56time2a* tm, time_t* t)
{
    struct tm tml;
    localtime_r(t, &tml);
    tm->msec = tml.tm_sec * 1000;
    tm->iv_min = tml.tm_min;
    tm->su_hour = tml.tm_hour;
    IEC_SET_SU(tm->su_hour, tml.tm_isdst);
    tm->wd_md = tml.tm_mday;
    IEC_SET_DOW(tm->wd_md, tml.tm_wday);
    tm->month = tml.tm_mon + 1;
    tm->year = tml.tm_year - 100;
}

/*
 * Convert IEC104 time <cp56time2a> to POSIX <struct tm>
 */
static void cp56time2a_to_tm(cp56time2a* tm, struct tm* tml) {
    tml->tm_sec = (int)(tm->msec / 1000);
    tml->tm_min = tm->iv_min & IECF_MIN;
    tml->tm_hour = tm->su_hour & IECF_HOUR;
    tml->tm_isdst = IEC_GET_SU(tm->su_hour);
    tml->tm_mday = tm->wd_md & IECF_MDAY;
    tml->tm_wday = IEC_GET_DOW(tm->wd_md);
    tml->tm_mon = (tm->month & IECF_MONTH) - 1;
    tml->tm_year = (tm->year & IECF_YEAR) + 100;
}

/*
 * Get current time as IEC104 time <cp56time2a>
 */
static void current_cp56time2a(cp56time2a* tm) {
    struct timeval tv;
    gettimeofday(&tv, NULL);
    tv_to_cp56time2a(tm, &tv);
}

/*
 * Convert IEC104 time <cp56time2a> to POSIX <struct timeval>
 */
static void cp56time2a_to_tv(cp56time2a* tm, struct timeval* tv) {
    struct tm tml;
    cp56time2a_to_tm(tm, &tml);
    tv->tv_sec = mktime(&tml);
    tv->tv_usec = (tm->msec % 1000) * 1000;
}

/*
*---------------------------------------------------------------------------------------------------------------
*= = CREATE ASDU FUNCTIONS
*---------------------------------------------------------------------------------------------------------------
*/

/*
 * Create ASDU header
 * Params:
 * buf:         destination buffer
 * len:         ptr to integer var to return header length
 * type:        ASDU type
 * num:         values count
 * sq:          single/queue (currently always single, i.e. =0)
 * cause:       cause of transmission
 * t:           test
 * pn:          positive/negative
 * ca:          common address of ASDU
 */
static void create_asdu_header(uint8_t* buf, int* len, uint8_t type,
    uint8_t num, uint8_t sq, uint8_t cause, uint8_t t, uint8_t pn,
    uint16_t ca) {
    data_unit_t unit;
    unit.type = type;
    unit.vsq = num;
    IEC_SET_SQ(unit.vsq, sq);
    unit.cause = cause;
    IEC_SET_T(unit.cause, t);
    IEC_SET_PN(unit.cause, pn);
    unit.oa = icb.c_oa;
    unit.ca = ca;
    int usize = icb.c_ca_len == 2 ? sizeof(data_unit_t) : sizeof(data_unit_t) - 1;
    memcpy(buf, &unit, usize);
    *len += usize;
}

/* C_IC_NA_1 - Interrogation command */
static void create_asdu_100(uint8_t* buf, int* len) {
    asdu100 type;
    uint32_t ioa = 0;
    type.qoi = 20;
    *(uint32_t*)buf = ioa;
    *(asdu100*)(buf + icb.c_ioa_len) = type;
    *len += icb.c_ioa_len + sizeof(asdu100);
}
/* C_CI_NA_1 - Counter interrogation command */
static void create_asdu_101(uint8_t* buf, int* len, uint8_t frz) {
    asdu101 type;
    uint32_t ioa = 0;
    type.qcc = 5;
    IEC_SET_FRZ(type.qcc, frz);
    *(uint32_t*)buf = ioa;
    *(asdu101*)(buf + icb.c_ioa_len) = type;
    *len += icb.c_ioa_len + sizeof(asdu101);
}
/* C_CS_NA_1 - 	Clock synchronization command */
static void create_asdu_103(uint8_t* buf, int* len) {
    asdu103 type;
    uint32_t ioa = 0;
    current_cp56time2a(&type.time);
    *(uint32_t*)buf = ioa;
    *(asdu103*)(buf + icb.c_ioa_len) = type;
    *len += icb.c_ioa_len + sizeof(asdu103);
}
/* C_TS_NA_1 - Test command */
static void create_asdu_104(uint8_t* buf, int* len) {
    asdu104 type;
    uint32_t ioa = 0;
    type.fbp = 0x55aa;
    *(uint32_t*)buf = ioa;
    *(asdu104*)(buf + icb.c_ioa_len) = type;
    *len += icb.c_ioa_len + sizeof(asdu104);
}
/* C_RP_NA_1 - Reset process command */
static void create_asdu_105(uint8_t* buf, int* len) {
    asdu105 type;
    uint32_t ioa = 0;
    type.qrp = 1;
    *(uint32_t*)buf = ioa;
    *(asdu105*)(buf + icb.c_ioa_len) = type;
    *len += icb.c_ioa_len + sizeof(asdu105);
}
/* C_TS_TA_1 - Test command with time tag CP56Time2a */
static void create_asdu_107(uint8_t* buf, int* len, uint16_t tsc) {
    asdu107 type;
    uint32_t ioa = 0;
    struct timeval tv;
    gettimeofday(&tv, NULL);
    tv_to_cp56time2a(&type.time, &tv);
    type.tsc = tsc;
    *(uint32_t*)buf = ioa;
    *(asdu107*)(buf + icb.c_ioa_len) = type;
    *len += icb.c_ioa_len + sizeof(asdu107);
}

/* C_SC_NA_1 - Single command */
static void create_asdu_45(uint8_t* buf, int* len, uint32_t ioa, iec_variant_t* v) {
    asdu45 type;
    cp56time2a tm;
    int size;
    current_cp56time2a(&tm);
    size = icb.c_ioa_len + sizeof(asdu45);
    type.sco = v->uint8;
    *(uint32_t*)buf = ioa;
    *(asdu45*)(buf + icb.c_ioa_len) = type;
    *len += size;
}
/* C_DC_NA_1 - Double command */
static void create_asdu_46(uint8_t* buf, int* len, uint32_t ioa, iec_variant_t* v) {
    asdu46 type;
    cp56time2a tm;
    int size;
    current_cp56time2a(&tm);
    size = icb.c_ioa_len + sizeof(asdu46);
    type.dco = v->uint8;
    *(uint32_t*)buf = ioa;
    *(asdu46*)(buf + icb.c_ioa_len) = type;
    *len += size;
}
/* C_RC_NA_1 - Regulating step command */
static void create_asdu_47(uint8_t* buf, int* len, uint32_t ioa, iec_variant_t* v) {
    asdu47 type;
    cp56time2a tm;
    int size;
    current_cp56time2a(&tm);
    size = icb.c_ioa_len + sizeof(asdu47);
    type.rco = v->uint8;
    *(uint32_t*)buf = ioa;
    *(asdu47*)(buf + icb.c_ioa_len) = type;
    *len += size;
}
/* C_SE_NA_1 - Set-point command, normalized value */
static void create_asdu_48(uint8_t* buf, int* len, uint32_t ioa, iec_variant_t* v) {
    asdu48 type;
    cp56time2a tm;
    int size;
    current_cp56time2a(&tm);
    size = icb.c_ioa_len + sizeof(asdu48);
    type.nva = v->int16;
    type.qos = 0;
    *(uint32_t*)buf = ioa;
    *(asdu48*)(buf + icb.c_ioa_len) = type;
    *len += size;
}
/* C_SE_NB_1 - Set-point command, scaled value */
static void create_asdu_49(uint8_t* buf, int* len, uint32_t ioa, iec_variant_t* v) {
    asdu49 type;
    cp56time2a tm;
    int size;
    current_cp56time2a(&tm);
    size = icb.c_ioa_len + sizeof(asdu49);
    type.sva = v->int16;
    type.qos = 0;
    *(uint32_t*)buf = ioa;
    *(asdu49*)(buf + icb.c_ioa_len) = type;
    *len += size;
}
/* C_SE_NC_1 - Set-point command, short floating point number */
static void create_asdu_50(uint8_t* buf, int* len, uint32_t ioa, iec_variant_t* v) {
    asdu50 type;
    cp56time2a tm;
    int size;
    current_cp56time2a(&tm);
    size = icb.c_ioa_len + sizeof(asdu50);
    type.r32 = v->real;
    type.qos = 0;
    *(uint32_t*)buf = ioa;
    *(asdu50*)(buf + icb.c_ioa_len) = type;
    *len += size;
}
/* C_BO_NA_1 - Bitstring of 32 bit command */
static void create_asdu_51(uint8_t* buf, int* len, uint32_t ioa, iec_variant_t* v) {
    asdu51 type;
    cp56time2a tm;
    int size;
    current_cp56time2a(&tm);
    size = icb.c_ioa_len + sizeof(asdu51);
    type.bsi = v->uint32;
    *(uint32_t*)buf = ioa;
    *(asdu51*)(buf + icb.c_ioa_len) = type;
    *len += size;
}

/* C_SC_TA_1 - Single command with time tag CP56Time2a */
static void create_asdu_58(uint8_t* buf, int* len, uint32_t ioa, iec_variant_t* v) {
    asdu58 type;
    int size;
    current_cp56time2a(&type.time);
    size = icb.c_ioa_len + sizeof(asdu58);
    type.sco = v->uint8;
    *(uint32_t*)buf = ioa;
    *(asdu58*)(buf + icb.c_ioa_len) = type;
    *len += size;
}
/* C_DC_TA_1 - Double command with time tag CP56Time2a */
static void create_asdu_59(uint8_t* buf, int* len, uint32_t ioa, iec_variant_t* v) {
    asdu59 type;
    int size;
    current_cp56time2a(&type.time);
    size = icb.c_ioa_len + sizeof(asdu59);
    type.dco = v->uint8;
    *(uint32_t*)buf = ioa;
    *(asdu59*)(buf + icb.c_ioa_len) = type;
    *len += size;
}
/* C_RC_TA_1 - Regulating step command with time tag CP56Time2a */
static void create_asdu_60(uint8_t* buf, int* len, uint32_t ioa, iec_variant_t* v) {
    asdu60 type;
    int size;
    current_cp56time2a(&type.time);
    size = icb.c_ioa_len + sizeof(asdu60);
    type.rco = v->uint8;
    *(uint32_t*)buf = ioa;
    *(asdu60*)(buf + icb.c_ioa_len) = type;
    *len += size;
}
/* C_SE_TA_1 - Set-point command, normalized value with time tag CP56Time2a */
static void create_asdu_61(uint8_t* buf, int* len, uint32_t ioa, iec_variant_t* v) {
    asdu61 type;
    int size;
    current_cp56time2a(&type.time);
    size = icb.c_ioa_len + sizeof(asdu61);
    type.nva = v->int16;
    type.qos = 0;
    *(uint32_t*)buf = ioa;
    *(asdu61*)(buf + icb.c_ioa_len) = type;
    *len += size;
}
/* C_SE_TB_1 - Set-point command, scaled value with time tag CP56Time2a */
static void create_asdu_62(uint8_t* buf, int* len, uint32_t ioa, iec_variant_t* v) {
    asdu62 type;
    int size;
    current_cp56time2a(&type.time);
    size = icb.c_ioa_len + sizeof(asdu62);
    type.sva = v->int16;
    type.qos = 0;
    *(uint32_t*)buf = ioa;
    *(asdu62*)(buf + icb.c_ioa_len) = type;
    *len += size;
}
/* C_SE_TC_1 - Set-point command, short floating point number with time tag CP56Time2a */
static void create_asdu_63(uint8_t* buf, int* len, uint32_t ioa, iec_variant_t* v) {
    asdu63 type;
    int size;
    current_cp56time2a(&type.time);
    size = icb.c_ioa_len + sizeof(asdu63);
    type.r32 = v->real;
    type.qos = 0;
    *(uint32_t*)buf = ioa;
    *(asdu63*)(buf + icb.c_ioa_len) = type;
    *len += size;
}
/* C_BO_TA_1 - Bitstring of 32 bit command with time tag CP56Time2a */
static void create_asdu_64(uint8_t* buf, int* len, uint32_t ioa, iec_variant_t* v) {
    asdu64 type;
    int size;
    current_cp56time2a(&type.time);
    size = icb.c_ioa_len + sizeof(asdu64);
    type.bsi = v->uint32;
    *(uint32_t*)buf = ioa;
    *(asdu64*)(buf + icb.c_ioa_len) = type;
    *len += size;
}

/* M_SP_NA_1 - Single point information */
static void create_asdu_1(uint8_t* buf, int* len, queue_item_t** queue, uint8_t num) {
    asdu1 type;
    int i, size;
    size = icb.c_ioa_len + sizeof(asdu1);
    iec_data_t* data;
    for (i = 0; i < num; i++) {
        data = &queue[i]->data;
        type.qds = (data->qds & 0xFE) | (data->value.int8 & 1);
        *(uint32_t*)buf = data->ioa;
        *(asdu1*)(buf + icb.c_ioa_len) = type;
        *len += size;
        buf += size;
    }
}
/* M_DP_NA_1 - Double point information */
static void create_asdu_3(uint8_t* buf, int* len, queue_item_t** queue, uint8_t num) {
    asdu3 type;
    int i, size;
    size = icb.c_ioa_len + sizeof(asdu3);
    iec_data_t* data;
    for (i = 0; i < num; i++) {
        data = &queue[i]->data;
        type.qds = (data->qds & 0xFC) | (data->value.int8 & 3);
        *(uint32_t*)buf = data->ioa;
        *(asdu3*)(buf + icb.c_ioa_len) = type;
        *len += size;
        buf += size;
    }
}
/* M_ST_NA_1 - Step position information */
static void create_asdu_5(uint8_t* buf, int* len, queue_item_t** queue, uint8_t num) {
    asdu5 type;
    int i, size;
    size = icb.c_ioa_len + sizeof(asdu5);
    iec_data_t* data;
    for (i = 0; i < num; i++) {
        data = &queue[i]->data;
        type.qds = data->qds;
        type.vti = data->value.uint8;
        *(uint32_t*)buf = data->ioa;
        *(asdu5*)(buf + icb.c_ioa_len) = type;
        *len += size;
        buf += size;
    }
}
/* M_BO_NA_1 - Bitstring of 32 bit */
static void create_asdu_7(uint8_t* buf, int* len, queue_item_t** queue, uint8_t num) {
    asdu7 type;
    int i, size;
    size = icb.c_ioa_len + sizeof(asdu7);
    iec_data_t* data;
    for (i = 0; i < num; i++) {
        data = &queue[i]->data;
        type.qds = data->qds;
        type.stcd = data->value.uint32;
        *(uint32_t*)buf = data->ioa;
        *(asdu7*)(buf + icb.c_ioa_len) = type;
        *len += size;
        buf += size;
    }
}
/* M_ME_NA_1 - Measured value, normalised value */
static void create_asdu_9(uint8_t* buf, int* len, queue_item_t** queue, uint8_t num) {
    asdu9 type;
    int i, size;
    size = icb.c_ioa_len + sizeof(asdu9);
    iec_data_t* data;
    for (i = 0; i < num; i++) {
        data = &queue[i]->data;
        type.qds = data->qds;
        type.mv = data->value.int16;
        *(uint32_t*)buf = data->ioa;
        *(asdu9*)(buf + icb.c_ioa_len) = type;
        *len += size;
        buf += size;
    }
}
/* M_ME_NB_1 - Measured value, scaled value */
static void create_asdu_11(uint8_t* buf, int* len, queue_item_t** queue, uint8_t num) {
    asdu11 type;
    int i, size;
    size = icb.c_ioa_len + sizeof(asdu11);
    iec_data_t* data;
    for (i = 0; i < num; i++) {
        data = &queue[i]->data;
        type.qds = data->qds;
        //SET_CP(type.qds, 0);
        type.mv = data->value.int16;
        *(uint32_t*)buf = data->ioa;
        *(asdu11*)(buf + icb.c_ioa_len) = type;
        *len += size;
        buf += size;
    }
}
/* M_ME_NC_1 - Measured value, short floating point number */
static void create_asdu_13(uint8_t* buf, int* len, queue_item_t** queue, uint8_t num) {
    asdu13 type;
    int i, size;
    size = icb.c_ioa_len + sizeof(asdu13);
    iec_data_t* data;
    for (i = 0; i < num; i++) {
        data = &queue[i]->data;
        type.qds = data->qds;
        type.mv = data->value.real;
        *(uint32_t*)buf = data->ioa;
        *(asdu13*)(buf + icb.c_ioa_len) = type;
        *len += size;
        buf += size;
    }
}
/* M_IT_NA_1 - Integrated total */
static void create_asdu_15(uint8_t* buf, int* len, queue_item_t** queue, uint8_t num) {
    asdu15 type;
    int i, size;
    size = icb.c_ioa_len + sizeof(asdu15);
    iec_data_t* data;
    for (i = 0; i < num; i++) {
        data = &queue[i]->data;
        type.qds = data->qds;
        type.bcr = data->value.int32;
        *(uint32_t*)buf = data->ioa;
        *(asdu15*)(buf + icb.c_ioa_len) = type;
        *len += size;
        buf += size;
    }
}

/* M_SP_TB_1 - Single point information with time tag CP56Time2a */ /* SQ=0 */
static void create_asdu_30(uint8_t* buf, int* len, queue_item_t** queue, uint8_t num) {
    asdu30 type;
    int i, size;
    size = icb.c_ioa_len + sizeof(asdu30);
    iec_data_t* data;
    for (i = 0; i < num; i++) {
        data = &queue[i]->data;
        type.qds = (data->qds & 0xFE) | (data->value.int8 & 1);
        tv_to_cp56time2a(&type.time, &data->tv);
        *(uint32_t*)buf = data->ioa;
        *(asdu30*)(buf + icb.c_ioa_len) = type;
        *len += size;
        buf += size;
    }
}
/* M_DP_TB_1 - Double point information with time tag CP56Time2a */
static void create_asdu_31(uint8_t* buf, int* len, queue_item_t** queue, uint8_t num) {
    asdu31 type;
    int i, size;
    size = icb.c_ioa_len + sizeof(asdu31);
    iec_data_t* data;
    for (i = 0; i < num; i++) {
        data = &queue[i]->data;
        type.qds = (data->qds & 0xFC) | (data->value.int8 & 3);
        tv_to_cp56time2a(&type.time, &data->tv);
        *(uint32_t*)buf = data->ioa;
        *(asdu31*)(buf + icb.c_ioa_len) = type;
        *len += size;
        buf += size;
    }
}
/* M_ST_TB_1 - Step position information with time tag CP56Time2a */
static void create_asdu_32(uint8_t* buf, int* len, queue_item_t** queue, uint8_t num) {
    asdu32 type;
    int i, size;
    size = icb.c_ioa_len + sizeof(asdu32);
    iec_data_t* data;
    for (i = 0; i < num; i++) {
        data = &queue[i]->data;
        type.qds = data->qds;
        type.vti = data->value.uint8;
        tv_to_cp56time2a(&type.time, &data->tv);
        *(uint32_t*)buf = data->ioa;
        *(asdu32*)(buf + icb.c_ioa_len) = type;
        *len += size;
        buf += size;
    }
}
/* M_BO_TB_1 - Bitstring of 32 bit with time tag CP56Time2a */
static void create_asdu_33(uint8_t* buf, int* len, queue_item_t** queue, uint8_t num) {
    asdu33 type;
    int i, size;
    size = icb.c_ioa_len + sizeof(asdu33);
    iec_data_t* data;
    for (i = 0; i < num; i++) {
        data = &queue[i]->data;
        type.qds = data->qds;
        type.stcd = data->value.uint32;
        tv_to_cp56time2a(&type.time, &data->tv);
        *(uint32_t*)buf = data->ioa;
        *(asdu33*)(buf + icb.c_ioa_len) = type;
        *len += size;
        buf += size;
    }
}
/* M_ME_TD_1 - Measured value, normalised value with time tag CP56Time2a */
static void create_asdu_34(uint8_t* buf, int* len, queue_item_t** queue, uint8_t num) {
    asdu34 type;
    int i, size;
    size = icb.c_ioa_len + sizeof(asdu34);
    iec_data_t* data;
    for (i = 0; i < num; i++) {
        data = &queue[i]->data;
        type.qds = data->qds;
        type.mv = data->value.int16;
        tv_to_cp56time2a(&type.time, &data->tv);
        *(uint32_t*)buf = data->ioa;
        *(asdu34*)(buf + icb.c_ioa_len) = type;
        *len += size;
        buf += size;
    }
}
/* M_ME_TE_1 - Measured value, scaled value with time tag CP56Time2a */
static void create_asdu_35(uint8_t* buf, int* len, queue_item_t** queue, uint8_t num) {
    asdu35 type;
    int i, size;
    size = icb.c_ioa_len + sizeof(asdu35);
    iec_data_t* data;
    for (i = 0; i < num; i++) {
        data = &queue[i]->data;
        type.qds = data->qds;
        //SET_CP(type.qds, 0);
        type.mv = data->value.int16;
        tv_to_cp56time2a(&type.time, &data->tv);
        *(uint32_t*)buf = data->ioa;
        *(asdu35*)(buf + icb.c_ioa_len) = type;
        *len += size;
        buf += size;
    }
}
/* M_ME_TF_1 - Measured value, short floating point number with time tag CP56Time2a */
static void create_asdu_36(uint8_t* buf, int* len, queue_item_t** queue, uint8_t num) {
    asdu36 type;
    int i, size;
    size = icb.c_ioa_len + sizeof(asdu36);
    iec_data_t* data;
    for (i = 0; i < num; i++) {
        data = &queue[i]->data;
        type.qds = data->qds;
        type.mv = data->value.real;
        tv_to_cp56time2a(&type.time, &data->tv);
        *(uint32_t*)buf = data->ioa;
        *(asdu36*)(buf + icb.c_ioa_len) = type;
        *len += size;
        buf += size;
    }
}
/* M_IT_TB_1 - Integrated total with time tag CP56Time2a */
static void create_asdu_37(uint8_t* buf, int* len, queue_item_t** queue, uint8_t num) {
    asdu37 type;
    int i, size;
    size = icb.c_ioa_len + sizeof(asdu37);
    iec_data_t* data;
    for (i = 0; i < num; i++) {
        data = &queue[i]->data;
        type.qds = data->qds;
        type.bcr = data->value.int32;
        tv_to_cp56time2a(&type.time, &data->tv);
        *(uint32_t*)buf = data->ioa;
        *(asdu37*)(buf + icb.c_ioa_len) = type;
        *len += size;
        buf += size;
    }
}

/*
 * SEND I-FRAME FUNCTIONS
 */

 /* C_IC_NA_1 - Interrogation command */
int iec_send_i_100(iec_connection_t* c, uint16_t ca, uint8_t cause)
{
    int /*cause, */len = 0;
    uint8_t buf[256];
    iec_socket_t* s = c->parent;
    //cause = (s->flags & _IECF_S_MASTER) ? IEC_CT_ACT : IEC_CT_ACTCON;
    create_asdu_header(buf, &len, 100, 1, 0, cause, 0, 0, ca);
    create_asdu_100(buf + len, &len);
    if (s->flags & _IECF_S_MASTER) c->time_ic = time(NULL);
    return iec_send_i(c, buf, len);
}
/* C_CI_NA_1 - Counter interrogation command */
int iec_send_i_101(iec_connection_t* c, uint16_t ca, uint8_t frz) {
    int cause, len = 0;
    uint8_t buf[256];
    iec_socket_t* s = c->parent;
    cause = (s->flags & _IECF_S_MASTER) ? IEC_CT_ACT : IEC_CT_ACTCON;
    create_asdu_header(buf, &len, 101, 1, 0, cause, 0, 0, ca);
    create_asdu_101(buf + len, &len, frz);
    return iec_send_i(c, buf, len);
}
/* C_RD_NA_1 - Read command */
int iec_send_i_102(iec_connection_t* c, uint16_t ca, uint32_t ioa) {
    int cause, len = 0;
    uint8_t buf[256];
    iec_socket_t* s = c->parent;
    cause = (s->flags & _IECF_S_MASTER) ? IEC_CT_ACT : IEC_CT_ACTCON;
    create_asdu_header(buf, &len, 102, 1, 0, cause, 0, 0, ca);
    *(uint32_t*)(buf + len) = ioa;
    len += icb.c_ioa_len;
    return iec_send_i(c, buf, len);
}
/* C_CS_NA_1 - 	Clock synchronisation command */
int iec_send_i_103(iec_connection_t* c, uint16_t ca) {
    int cause, len = 0;
    uint8_t buf[256];
    iec_socket_t* s = c->parent;
    cause = (s->flags & _IECF_S_MASTER) ? IEC_CT_ACT : IEC_CT_ACTCON;
    create_asdu_header(buf, &len, 103, 1, 0, cause, 0, 0, ca);
    create_asdu_103(buf + len, &len);
    return iec_send_i(c, buf, len);
}
/* C_TS_NA_1 - Test command */
int iec_send_i_104(iec_connection_t* c, uint16_t ca) {
    int cause, len = 0;
    uint8_t buf[256];
    iec_socket_t* s = c->parent;
    cause = (s->flags & _IECF_S_MASTER) ? IEC_CT_ACT : IEC_CT_ACTCON;
    create_asdu_header(buf, &len, 104, 1, 0, cause, 0, 0, ca);
    create_asdu_104(buf + len, &len);
    return iec_send_i(c, buf, len);
}
/* C_RP_NA_1 - Reset process command */
int iec_send_i_105(iec_connection_t* c, uint16_t ca) {
    int cause, len = 0;
    uint8_t buf[256];
    iec_socket_t* s = c->parent;
    cause = (s->flags & _IECF_S_MASTER) ? IEC_CT_ACT : IEC_CT_ACTCON;
    create_asdu_header(buf, &len, 105, 1, 0, cause, 0, 0, ca);
    create_asdu_105(buf + len, &len);
    return iec_send_i(c, buf, len);
}
/* C_TS_TA_1 - Test command with time tag CP56Time2a */
int iec_send_i_107(iec_connection_t* c, uint16_t ca, uint16_t tsc) {
    int cause, len = 0;
    uint8_t buf[256];
    iec_socket_t* s = c->parent;
    cause = (s->flags & _IECF_S_MASTER) ? IEC_CT_ACT : IEC_CT_ACTCON;
    create_asdu_header(buf, &len, 107, 1, 0, cause, 0, 0, ca);
    create_asdu_107(buf + len, &len, tsc);
    return iec_send_i(c, buf, len);
}

// Send data from SLAVE to MASTER
/* M_SP_NA_1 - Single point information */
/* M_DP_NA_1 - Double point information */
/* M_ST_NA_1 - Step position information */
/* M_BO_NA_1 - Bitstring of 32 bit */
/* M_ME_NA_1 - Measured value, normalized value */
/* M_ME_NB_1 - Measured value, scaled value */
/* M_ME_NC_1 - Measured value, short floating point number */
/* M_IT_NA_1 - Integrated total */
/* M_SP_TB_1 - Single point information with time tag CP56Time2a */
/* M_DP_TB_1 - Double point information with time tag CP56Time2a */
/* M_ST_TB_1 - Step position information with time tag CP56Time2a */
/* M_BO_TB_1 - Bitstring of 32 bit with time tag CP56Time2a */
/* M_ME_TD_1 - Measured value, normalized value with time tag CP56Time2a */
/* M_ME_TE_1 - Measured value, scaled value with time tag CP56Time2a */
/* M_ME_TF_1 - Measured value, short floating point number with time tag CP56Time2a */
/* M_IT_TB_1 - Integrated total with time tag CP56Time2a */
typedef void (*create_asdu_func1_t)(uint8_t*, int*, queue_item_t**, uint8_t);
static create_asdu_func1_t create_asdu_func1[2][IECT_MAX + 1] = {
    //     single          double          step            bit32            normalized      scaled          float           integrated
    {NULL, create_asdu_1,  create_asdu_3,  create_asdu_5,  create_asdu_7,   create_asdu_9,  create_asdu_11, create_asdu_13, create_asdu_15},
    {NULL, create_asdu_30, create_asdu_31, create_asdu_32, create_asdu_33,  create_asdu_34, create_asdu_35, create_asdu_36, create_asdu_37}
};

// Send command from MASTER to SLAVE
/* C_SC_NA_1 - Single command */
/* C_DC_NA_1 - Double command */
/* C_RC_NA_1 - Regulating step command */
/* C_SE_NA_1 - Set-point command, normalized value */
/* C_SE_NB_1 - Set-point command, scaled value */
/* C_SE_NC_1 - Set-point command, short floating point number */
/* C_SC_TA_1 - Single command with time tag CP56Time2a */
/* C_DC_TA_1 - Double command with time tag CP56Time2a */
/* C_RC_TA_1 - Regulating step command with time tag CP56Time2a */
/* C_SE_TA_1 - Set-point command, normalized value with time tag CP56Time2a */
/* C_SE_TB_1 - Set-point command, scaled value with time tag CP56Time2a */
/* C_SE_TC_1 - Set-point command, short floating point number with time tag CP56Time2a */
typedef void (*create_asdu_func2_t)(uint8_t*, int*, uint32_t, iec_variant_t*);
static create_asdu_func2_t create_asdu_func2[2][IECT_MAX] = {
    //     single          double          step            bit32            normalized      scaled          float           
    {NULL, create_asdu_45, create_asdu_46, create_asdu_47, create_asdu_51,  create_asdu_48, create_asdu_49, create_asdu_50},
    {NULL, create_asdu_58, create_asdu_59, create_asdu_60, create_asdu_64,  create_asdu_61, create_asdu_62, create_asdu_63}
};

/*
 * Return ptr to sector structure by socket and common address of ASDU
 */
static iec_sector_t* iec_socket_get_sector(iec_socket_t* s, const uint16_t ca)
{
    if (s == NULL || ca == 0)
    {
        errno = EINVAL;
        return NULL;
    }
    int i;
    for (i = 0; i < s->sector_count; i++) if (s->sectors[i]->ca == ca) return s->sectors[i];
    return NULL;
}

/*
 * Write data to socket
 * Params:
 * s:           Socket structure
 * c:           Connection structure
 * type:        ASDU type
 * with_time:   With time (1) or without time (0)
 * ca:          Common address of ASDU
 * queue:       Ptr to values list to send
 * num:         number of values
 * cause:       Cause of transmission
 *
 * Return 0 if success, otherwise -1
 */
static int iec_send_data(iec_socket_t* s, iec_connection_t* c, uint8_t type, uint8_t with_time, uint16_t ca, queue_item_t** queue, uint8_t num, uint8_t cause)
{
    int len = 0;
    static uint8_t buf[256];
    iec_sector_t* sector;
    create_asdu_func1_t f;
    if (type > IECT_MAX
        || (f = create_asdu_func1[with_time][type]) == NULL
        || (sector = iec_socket_get_sector(s, ca)) == NULL) {
        errno = EINVAL;
        return -1;
    }
    type = asdu_types[with_time][type];
    if (c == NULL) {
        errno = EINVAL;
        return -1;
    }
    if (c->time_stop) {
        errno = EPIPE;
        return -1;
    }
    if (num == 0 || num > iec_sector_max_num(type, 0))
    {
        errno = EINVAL;
        return -1;
    }
    //if (check_server_num(c, type, num, 0) < 0) return -1;
    create_asdu_header(buf, &len, type, num, 0, cause, 0, 0, ca);
    f(buf + len, &len, queue, num);
    LOG(IEC_LOG_ALL, IO_ASDU, 'T', get_ctx_name(cause), type, ca, num, connection_info(c));
    return iec_send_i(c, buf, len);
}

/*
*---------------------------------------------------------------------------------------------------------------
*= = CREATE ASDU FUNCTIONS
*---------------------------------------------------------------------------------------------------------------
*/

/* Trying to establish client socket connection */
static int iec_client_open(iec_socket_t* s)
{
    if (s->client.connecting) return 0;
    s->client.connecting = 1;
    if (client_try_open(s->client.client) < 0)
    {
        LOG(IEC_LOG_ERROR, ERR_CLIENT_OPEN, errno, s->index, strerror(errno));
        s->client.connection.time_stop = time(NULL);
        s->client.connecting = 0;
        return -1;
    }
    return 0;
}

/* Disconnect client socket connection */
static void iec_client_close(iec_socket_t* s)
{
    client_close(s->client.client);
    s->client.connecting = 0;
}

static inline int valid_type(int type)
{
    return (type < 1 || type > 8) ? 0 : iec104_types[type - 1];
}

/* Free synchronization object */
static void iec_sync_free(void)
{
    sync_free(&icb.iec_sync);
}

/* Return IEC104 connection structure by index of server socket */
static iec_connection_t* iec_server_connection(iec_socket_t* s, int index)
{
    connection_t c;
    return (c = server_get_connection(s->server.server, index)) == NULL
        ? NULL
        : (iec_connection_t*)connection_get_data(c);
}

/*
 * Send data queue if needed
 */
static void send_queue(iec_socket_t* s, iec_connection_t* c)
{
    int j, with_time, num, m, n, max, size, ca;
    queue_item_t* q;
    uint8_t cause, type;
    uint16_t nums[64];
    queue_item_t* queue[64];
    while (c->queue->size) {
        q = &c->queue->queue[0];
        type = q->data.type;
        ca = q->ca;
        cause = q->data.cause;
        // if cause = IEC_CT_INTROGEN, then send data w/o time
#define WITH_TIME (cause != IEC_CT_INTROGEN && (q->data.flags & _IECF_WITH_TIME) ? 1 : 0)
        with_time = WITH_TIME;
        max = iec_sector_max_num(asdu_types[with_time][type], 0);
        nums[0] = 0;
        for (j = num = 1; j < c->queue->size && num < max; j++)
        {
            q = &c->queue->queue[j];
            if (q->ca == ca
                && q->data.type == type
                && WITH_TIME == with_time
                && q->data.cause == cause)
            {
                nums[num++] = j;
            }
        }
#undef WITH_TIME
        size = asdu_type_sizes[type & 0x7f];
        if (size == 0) continue;        // ???
        for (j = 0; j < num; j++)
            queue[j] = &c->queue->queue[nums[j]];
        iec_send_data(s, c, type, with_time, ca, queue, num, cause);
        for (j = m = n = 0; j < c->queue->size; j++)
        {
            if (n == num || j != nums[n]) {
                c->queue->queue[m++] = c->queue->queue[j];
            }
            else
                n++;
        }
        c->queue->size -= num;
    }
}

/*
 * Count of maximum values of selected type that can be send in one frame
 */
static int iec_sector_max_num(uint8_t type, uint8_t sq)
{
    int size = asdu_sizes[type];
    if (size == 255) return 0;
    int res = sq
        ? ((ASDU_MAX_DATA_LEN - icb.c_ioa_len) / size)
        : (ASDU_MAX_DATA_LEN / (size + icb.c_ioa_len));
    return res;
}

static void process_slave_connection(iec_socket_t* s, iec_connection_t* c)
{
    send_queue(s, c);
}

/* Server socket timer function */
static void process_server(iec_socket_t* s, time_t t)
{
    int i, count;
    iec_connection_t* c;
    if (icb.socket_count == 0)
    {
        return;
    }
    server_t server = s->server.server;
    count = server_get_count(server);

    if (s->flags & _IECF_S_MASTER)
    {
        for (i = 0; i < count; i++)
        {
            c = iec_server_connection(s, i);
            if (c == NULL || !c->started) continue;
            if (c->time_t1 && t - c->time_t1 >= s->is.t1)
            {
                connection_close(c->connection);
                continue;
            }
            if (s->is.k > 0 && c->n_out_i > s->is.k)
            {
                connection_close(c->connection);
                continue;
            }
            if (t - c->time_t3 >= s->is.t3)
            {
                iec_send_u(c, TESTFR_ACT);
                c->time_t1 = c->time_t3 = t;
                continue;
            }
            if (t - c->time_write >= s->is.t2)
            {
                iec_send_s(c);
                continue;
            }
            if (IEC_TIMEOUT_IC && c->time_ic && t - c->time_ic > IEC_TIMEOUT_IC)
            {
                iec_send_i_100(c, CA_BROADCAST(s), IEC_CT_ACT);
                continue;
            }
        }
        return;
    }
    /*
    if (timer % 10 == 0)
        check_nt_timeout(s, t);
     */
    if (count == 0) return;
    icb.c_ca_len = s->is.ca_len;
    icb.c_ioa_len = s->is.ioa_len;
    icb.c_oa = s->is.oa;
    for (i = 0; i < count; i++)
    {
        c = iec_server_connection(s, i);
        if (c == NULL || !c->started) continue;
        process_slave_connection(s, c);
        if (t - c->time_write >= s->is.t2)
        {
            iec_send_s(c);
            continue;
        }

        if (c->time_t1 && t - c->time_t1 >= s->is.t1)
        {
            //iec_send_u(c, TESTFR_ACT);
            connection_close(c->connection);
            continue;
        }

        if (s->is.k > 0 && c->n_out_i > s->is.k)
        {
            connection_close(c->connection);
            continue;
        }

        if (t - c->time_t3 >= s->is.t3)
        {
            iec_send_u(c, TESTFR_ACT);
            c->time_t1 = c->time_t3 = t;
            continue;
        }
    }
}

/*
 * Process command <C_RD_NA_1> (read)
 */
static void iec_sector_enqueue_data1(iec_connection_t* c, iec_sector_t* sector, uint32_t ioa)
{
    queue_item_t q;
    iec_data_t* data;
    if (ioa > sector->max_ioa || (data = iec_sector_data(sector, ioa)) == NULL) {
        iec_send_i_102(c, sector->ca, IEC_CT_UK_IOA);
        return;
    }
    if (data->type == 0 || (data->type & 0x80)) {
        iec_send_i_102(c, sector->ca, IEC_CT_UK_TYPE);
        return;
    }
    q.ca = sector->ca;
    q.data = *data;
    q.data.cause = IEC_CT_REQ;
    iec_enqueue_item(c, &q);
}

/* Set NON_TOPIC flag if there is timeout */
static void process_sectors(time_t t)
{
    int i, j;
    iec_sector_t* sector;
    iec_data_t* data;
    for (i = 0; i < icb.sector_max; i++)
    {
        if ((sector = icb.sectors[i]) != NULL)
        {
            for (j = 0, data = &sector->data[0]; j < sector->count; j++, data++)
            {
                if ((data->qds & (IECF_NT | IECF_SB | IECF_BL | IECF_IV)) == 0 && t - data->tv.tv_sec > IEC_TIMEOUT_NT)
                {
                    data->qds |= IECF_NT;
                    iec_sector_enqueue_data(sector, data->ioa, &data->tv, IEC_CT_SPONT);
                }
            }
        }
    }
}

/* Client socket timer function */
static void process_client(iec_socket_t* s, time_t t/*, int timer*/)
{
    if (s->client.connection.time_stop)
    {
        if (t - s->client.connection.time_stop > IEC_TIMEOUT_CONNECT && s->client.connecting == 0)
        {
            iec_client_open(s);
        }
        return;
    }
    if (s->flags & _IECF_S_MASTER)
    {
        if (s->client.connection.time_t1 && t - s->client.connection.time_t1 >= s->is.t1)
        {
            iec_client_close(s);
            return;
        }
        if (s->is.k > 0 && s->client.connection.n_out_i > s->is.k)
        {
            iec_client_close(s);
            return;
        }
        if (t - s->client.connection.time_t3 >= s->is.t3)
        {
            iec_send_u(&s->client.connection, TESTFR_ACT);
            s->client.connection.time_t1 = s->client.connection.time_t3 = t;
            return;
        }
        if (t - s->client.connection.time_write >= s->is.t2)
        {
            iec_send_s(&s->client.connection);
            return;
        }
        if (IEC_TIMEOUT_IC && s->client.connection.time_ic && t - s->client.connection.time_ic > IEC_TIMEOUT_IC)
        {
            iec_send_i_100(&s->client.connection, CA_BROADCAST(s), IEC_CT_ACT);
        }
        return;
    }
    iec_connection_t* c = &s->client.connection;
    if (t - c->time_write >= s->is.t2)
    {
        iec_send_s(c);
        goto next;
    }
    if (c->time_t1 && t - c->time_t1 >= s->is.t1)
    {
        //iec_send_u(c, TESTFR_ACT);
        iec_client_close(s);
        goto next;
    }
    if (t - c->time_t3 >= s->is.t3)
    {
        iec_send_u(c, TESTFR_ACT);
        c->time_t1 = c->time_t3 = t;
        goto next;
    }
next:
    /*
    if (timer % 10 == 0)
        check_nt_timeout(s, t);
    */
    process_slave_connection(s, c);
}

/* General timer function, calls appropriate timer functions for all existing sockets */
static void iec_timer(void)
{
    static int timer = 0;
    int i;
    time_t t = time(NULL);
    timer++;
    //sync_lock(&icb.iec_sync);
    for (i = 0; i < icb.socket_count; i++)
    {
        iec_socket_t* s = icb.sockets[i];
        if (s->sector_count == 0)
        {
            continue;
        }

        if (icb.sockets[i]->flags & _IECF_S_CLIENT)
        {
            process_client(icb.sockets[i], t/*, timer*/);
        }
        else
        {
            process_server(icb.sockets[i], t/*, timer*/);
        }
    }
    if (timer % 10 == 0)
        process_sectors(t);
    if (icb.iec_timer_func != NULL)
        icb.iec_timer_func(t);
    //sync_unlock(&icb.iec_sync);
}

/*
 * Default callback command function (do nothing)
 */
static int iec_default_cmd_func(const uint16_t ca, uint32_t ioa, iec_variant_t* value)
{
    return iec_object_set_value(ca, ioa, value, NULL, NULL);
}

/*
 * Default event listener function
 */
static void iec_default_event_listener(const char* msg)
{
#ifdef __INSIGHT_OS_LINUX__
    static int file = -1;
    static struct timeval tv = { 0, 0 };
    static int old_day = -1;
    char buf[256];
    if (msg == NULL) {
        if (file >= 0) {
            close(file);
            file = -1;
        }
        return;
    }
    gettimeofday(&tv, NULL);
    struct tm tm;
    localtime_r(&tv.tv_sec, &tm);
    if (old_day != tm.tm_mday) {
        old_day = tm.tm_mday;
        if (file >= 0) {
            close(file);
            file = -1;
        }

        // Delete old files
        DIR* dir;
        struct dirent* de;
        if ((dir = opendir(default_log_dir)) == NULL) return;
        int dfd = dirfd(dir);
        time_t timeout = 3 * 31 * 86400;
        while ((de = readdir(dir)) != NULL) {
            if (de->d_type != DT_REG) continue;
            struct stat stat;
            if (fstatat(dfd, de->d_name, &stat, 0) < 0) continue;
            if (tv.tv_sec - stat.st_mtim.tv_sec > timeout)
                unlinkat(dfd, de->d_name, 0);
        }
        closedir(dir);
    }

    if (file < 0) {
        strcpy(buf, default_log_dir);
        int len = strlen(buf);
        strftime(buf + len, sizeof(buf) - len, "/%Y-%m-%d.log", &tm);
        file = open(buf, O_WRONLY | O_CREAT | O_APPEND, 0666);
        if (file < 0) {
            icb.iec_event_listener = NULL;
            return;
        }
    }

    strftime(buf, sizeof(buf), "%Y-%m-%d %H:%M:%S.", &tm);
    sprintf(&buf[strlen(buf)], "%03d ", (int)(tv.tv_usec / 1000));
    strcat(buf, msg);
    int len = strlen(buf);
    if (buf[len - 1] != '\n') buf[len++] = '\n';
    write(file, buf, len);
#endif
}


/*
 * General initialization: memory allocation, common synchronization object and timer creation
 * Params:
 * sector_max:  maximum number of sectors
 * socket_max:  maximum number of sockets
 * is:          ptr to common IEC104 settings structure
 */
int iec_init(int sector_max, int socket_max, iec_settings_t* is, syncobj_t* syncobj)
{
    memset(&icb, 0, sizeof(icb));
    icb.errormsg[0] = 0;

    if (sector_max < 1
        || socket_max < 1
        || ((is != NULL)
            && (/*st->ca_len == 0 ||*/ is->ca_len > 2 || /*st->ioa_len == 0 ||*/ is->ioa_len > 3)))
    {
        errno = EINVAL;
        SET_ERROR(ERR_INIT, errno, strerror(errno));
        return -1;
    }

    int size = sector_max + socket_max;
    icb.buf = calloc(size, sizeof(void*));
    if (icb.buf == NULL)
    {
        return -1;
    }
    
    icb.sector_max = sector_max;
    icb.socket_max = socket_max;
    icb.sectors = (iec_sector_t**)icb.buf;
    icb.sockets = (iec_socket_t**)((intptr_t)icb.buf + sector_max * sizeof(void*));
    
    if (is == NULL)
    {
        icb.is.oa = 0;
        icb.is.k = CONST_K;
        icb.is.w = CONST_W;
        icb.is.t0 = CONST_T0;
        icb.is.t1 = CONST_T1;
        icb.is.t2 = CONST_T2;
        icb.is.t3 = CONST_T3;
        icb.is.ca_len = CONST_CA_LEN;
        icb.is.ioa_len = CONST_IOA_LEN;
    }
    else
    {
        //if (is->k == 0) is->k = CONST_K;
        if (is->w == 0) is->w = CONST_W;
        if (is->t0 == 0) is->t0 = CONST_T0;
        if (is->t1 == 0) is->t1 = CONST_T1;
        if (is->t2 == 0) is->t2 = CONST_T2;
        if (is->t3 == 0) is->t3 = CONST_T3;
        if (is->ca_len == 0) is->ca_len = CONST_CA_LEN;
        if (is->ioa_len == 0) is->ioa_len = CONST_IOA_LEN;
        icb.is = *is;
    }

    if (syncobj == NULL)
    {
        if (sync_init(&icb.iec_sync) < 0)
        {
            SET_ERROR(ERR_INIT, errno, strerror(errno));
            free(icb.buf);
            icb.buf = NULL;
            return -1;
        }
    }
    else
    {
        icb.iec_sync = *syncobj;
    }
    
    icb.iec_timer = timer_new(iec_timer, IEC_TIMEOUT_TIMER, &icb.iec_sync);
    if (icb.iec_timer == NULL)
    {
        SET_ERROR(ERR_INIT, errno, strerror(errno));
        iec_sync_free();
        free(icb.buf);
        icb.buf = NULL;
        return -1;
    }

    icb.iec_cmd_func = iec_default_cmd_func;
    icb.iec_event_listener = iec_default_event_listener;

    return 0;
}

/* Delete all initialized objects */
void iec_free()
{
    if (icb.iec_event_listener == iec_default_event_listener)
    {
        iec_default_event_listener(NULL);
    }

    if (icb.buf == NULL) return;
    int i;
    iec_socket_t* s;
    while (icb.socket_count)
    {
        s = icb.sockets[--icb.socket_count];
        if (s->flags & _IECF_S_CLIENT) {
            client_close(s->client.client);
            client_wait(s->client.client);
            client_free(s->client.client);
        }
        else {
            server_stop(s->server.server);
            server_wait(s->server.server);
            if (s->server.access_list != NULL) {
                free(s->server.access_list);
                s->server.access_list = NULL;
                s->server.access_count = 0;
            }
            server_free(s->server.server);
        }
        free(s);
    }
    timer_free(icb.iec_timer);
    iec_sync_free();
    for (i = 0; i < icb.sector_max; i++) {
        if (icb.sectors[i] != NULL) free(icb.sectors[i]);
    }
    free(icb.buf);
    char save_err[sizeof(icb.errormsg)];
    strncpy(save_err, icb.errormsg, sizeof(icb.errormsg));
    memset(&icb, 0, sizeof(icb));
    strcpy(icb.errormsg, save_err);
}

/*
 * Start IEC104 processing
 * Connects all clients and starts listening of all servers, starts timer
 * Return 0 if success, otherwise -1
 */
int iec_start()
{
    int i, error;
    iec_socket_t* s;
    if (icb.iec_event_listener == iec_default_event_listener)
    {
        struct stat st;
        if (stat(default_log_dir, &st) < 0)
        {
#ifdef MINGW
            if (errno != ENOENT || mkdir(default_log_dir) < 0)
#else
            if (errno != ENOENT || mkdir(default_log_dir, 0777) < 0)
#endif
                icb.iec_event_listener = NULL;
        }
        else
        {
            if ((st.st_mode & S_IFMT) != S_IFDIR)
                icb.iec_event_listener = NULL;
        }
    }
    EVENT(EVT_STARTING);
    if (icb.socket_count == 0)
    {
        errno = EINVAL;
        SET_ERROR(ERR_IEC_START_EMPTY);
        return -1;
    }

    for (i = 0; i < icb.socket_count; i++)
    {
        s = icb.sockets[i];
        if (s->sector_count == 0)
        {
            errno = EINVAL;
            SET_ERROR(ERR_IEC_START_NOSEC, i);
            return -1;
        }
    }

    for (i = 0; i < icb.socket_count; i++)
    {
        s = icb.sockets[i];
        if (s->flags & _IECF_S_CLIENT) continue;
        if (server_start(s->server.server) < 0)
        {
            error = errno;
            SET_ERROR(ERR_SERVER_START, errno, i, s->port, strerror(errno));
            for (--i; i >= 0; i--) {
                if (s->flags & _IECF_S_CLIENT) continue;
                s = icb.sockets[i];
                server_stop(s->server.server);
            }
            errno = error;
            return -1;
        }
    }

    for (i = 0; i < icb.socket_count; i++)
    {
        s = icb.sockets[i];
        if (s->flags & _IECF_S_CLIENT)
        {
            iec_client_open(s);
        }
    }

    if (timer_start(icb.iec_timer) < 0)
    {
        error = errno;
        iec_stop();
        errno = error;
        SET_ERROR(ERR_TIMER_START, errno, strerror(errno));
        return -1;
    }
    icb.flags |= IECF_RUNNING;
    return 0;
}

/* Stop IEC104 processing: close all connections, servers and stop timer */
void iec_stop()
{
    int i;
    iec_socket_t* s;
    icb.flags |= IECF_STOPPING;
    for (i = 0; i < icb.socket_count; i++)
    {
        s = icb.sockets[i];
        if (s->flags & _IECF_S_CLIENT)
            iec_client_close(s);
        else
            server_stop(s->server.server);
    }
    timer_stop(icb.iec_timer);
    sleep(1);
    EVENT(EVT_STOPPED);
    icb.flags &= ~IECF_RUNNING;
}

void iec_wait(void)
{
    if (icb.flags & IECF_RUNNING)
    {
        timer_wait(icb.iec_timer);
    }
}

/*
 * Parsing received ASDU frame
 * Params:
 * oi:          Ptr to object info, filled with common frame data
 * obj:         Ptr to IEC104 data list; on return filled with one or more received values
 * count:       Ptr to count, on return filled with received values count
 * buf:         Source buffer
 * len:         Received bytes count
 *
 * Return:
 *              0: success
 *              -2: invalid or unsupported ASDU type
 *              -1: any other error occured
 */
static int parse_asdu(object_unit_t* oi, iec_asdu_t* obj, int* count, uint8_t* buf, size_t len) {
    int i, size, hsize;
    data_unit_t* hptr;
    hptr = (data_unit_t*)buf;
    *count = hptr->vsq & IECF_NUM;
    oi->type = hptr->type;
    oi->cause = hptr->cause & IECF_CAUSE;
    oi->test = IEC_GET_T(hptr->cause);
    oi->pn = IEC_GET_PN(hptr->cause);
    if (icb.c_ca_len == 2) {
        oi->ca = hptr->ca;
        hsize = sizeof(data_unit_t);
    }
    else { // == 1
        oi->ca = hptr->ca & 0xff;
        hsize = sizeof(data_unit_t) - 1;
    }
    if ((size = asdu_sizes[oi->type]) == 255)
        return -2;
    if (IEC_GET_SQ(hptr->vsq) == 0) {
        int step = size + icb.c_ioa_len;
        if (step * *count + hsize > len)
            return -1;
        buf += hsize;
        for (i = 0; i < *count; i++, buf += size) {
            switch (icb.c_ioa_len) {
            case 1:
                obj->ioa = (*(uint16_t*)buf) & 0xff;
                break;
            case 2:
                obj->ioa = *(uint16_t*)buf;
                break;
            default:
                obj->ioa = (*(uint32_t*)buf) & 0xffffff;
                break;
            }
            buf += icb.c_ioa_len;
            memmove(&obj->type1, buf, size);
            obj = (iec_asdu_t*)((char*)obj + size + 4);
        }
    }
    else {
        if (size * *count + hsize > len)
            return -1;
        uint32_t ioa;
        buf += hsize;
        switch (icb.c_ioa_len) {
        case 1:
            ioa = (*(uint16_t*)buf) & 0xff;
            break;
        case 2:
            ioa = *(uint16_t*)buf;
            break;
        default:
            ioa = (*(uint32_t*)buf) & 0xffffff;
            break;
        }
        buf += icb.c_ioa_len;
        for (i = 0; i < *count; i++, buf += size) {
            obj->ioa = ioa++;
            memmove(&obj->type1, buf, size);
            obj = (iec_asdu_t*)((char*)obj + size + 4);
        }
    }
    return 0;
}
/*
 * Check sequence integrity for S-frame
 */
static int check_sequense_s(iec_connection_t* c)
{
    if (c->seq * 2 >= c->apdu_s.ack) return 0;
    LOG(IEC_LOG_ERROR, ERR_SEQUENCE_VIOLATION, 'S', connection_info(c));
    return -1;
}

/*
 * Check sequence integrity for I-frame
 */
static int check_sequense_i(iec_connection_t* c)
{
    if (c->seq * 2 >= c->apdu_i.ack) return 0;
    LOG(IEC_LOG_ERROR, ERR_SEQUENCE_VIOLATION, 'I', connection_info(c));
    return -1;
}

/*
 * iec_data_t -> info_object_t
 */
static void export_io(iec_data_t* data, info_object_t* info)
{
    info->type = data->type;
    info->qds = data->qds;
    info->tv = data->tv;
    info->userdata = data->userdata;
    info->value = data->value;
}

/*
 * Send all data when Interrogation Command received
 */
static void process_slave_ic(iec_socket_t* s, iec_connection_t* c)
{
    int i;
    for (i = 0; i < s->sector_count; i++)
    {
        iec_slave_enqueue_sector(c, s->sectors[i]);
    }
}

/*
 * Reading and parsing input data by MASTER
 */
static int master_read(iec_connection_t* c)
{
    int i, count;
    char buf[512];
    object_unit_t oi;
    iec_data_t* data;
    iec_sector_t* sector;
    iec_socket_t* s = c->parent;
    icb.c_ca_len = s->is.ca_len;
    icb.c_ioa_len = s->is.ioa_len;
    icb.c_oa = s->is.oa;
    if (c->len == 0) return 0;
    while (1) {
        if (c->apdu_u.start != APDU_START)
        {
            hexdump(&c->apdu_u.start, c->len, 1);
            c->e_in++;
            LOG(IEC_LOG_ERROR, ERR_START_BYTE, connection_info(c));
            return -1;
        }
        if (c->apdu_u.len > c->len - 2)
            return 1;
        hexdump(c->buffer, c->apdu_u.len + 2, 1);
        if (c->apdu_u.cb & 1) {
            if (c->apdu_u.len != 4) {
                c->e_in++;
                LOG(IEC_LOG_ERROR, IO_TYPE_U, 'R', UNKNOWN, connection_info(c));
                return -1;
            }
            else if (c->apdu_u.cb & 2) {
                // U-frame
                LOG(IEC_LOG_ALL, IO_TYPE_U, 'R', get_u_name(c->apdu_u.cb & 0xfc), connection_info(c));
                LOG2(IEC_LOG_U, IO_TYPE_U, 'R', get_u_name(c->apdu_u.cb & 0xfc), connection_info(c));
                switch (c->apdu_u.cb & 0xfc) {
                case STARTDT_ACT:
                    c->n_in++;
                    iec_send_u(c, STARTDT_CON);
                    c->started = 1;
                    break;
                case TESTFR_ACT:
                    c->n_in++;
                    iec_send_u(c, TESTFR_CON);
                    break;
                case STOPDT_ACT:
                    c->n_in++;
                    iec_send_u(c, STOPDT_CON);
                    c->started = 0;
                    return -1;
                    break;
                case STARTDT_CON:
                    c->time_t3 = c->time_read;
                    c->time_t1 = 0;
                    if (s->flags & _IECF_S_IC_REQ)
                        iec_send_i_100(c, CA_BROADCAST(s), IEC_CT_ACT);
                    break;
                case TESTFR_CON:
                    c->time_t3 = c->time_read;
                    c->time_t1 = 0;
                    break;
                case STOPDT_CON:
                    c->time_t3 = c->time_read;
                    c->time_t1 = 0;
                    break;
                default:
                    LOG(IEC_LOG_ERROR, ERR_INVALID_CMD, c->apdu_u.cb & 0xfc, connection_info(c));
                    c->e_in++;
                    break;
                }
            }
            else {
                // S-frame
                if (check_sequense_s(c) < 0) return -1;
                c->time_t3 = c->time_read;
                c->time_t1 = 0;
                c->n_in++;
                c->apdu_s.ack >>= 1;
                c->n_out_i = 0;
                LOG(IEC_LOG_ALL, IO_TYPE_S, 'R', c->apdu_i.ack, connection_info(c));
                LOG2(IEC_LOG_S, IO_TYPE_S, 'R', c->apdu_i.ack, connection_info(c));
            }
        }
        else {
            // I-frame
            if (check_sequense_i(c) < 0) return -1;
            c->time_t3 = c->time_read;
            c->time_t1 = 0;
            c->apdu_i.seq >>= 1;
            c->apdu_i.ack >>= 1;
            LOG(IEC_LOG_ALL, IO_TYPE_I, 'R', c->apdu_i.len - 4, c->apdu_i.ack, c->apdu_i.seq, connection_info(c));
            LOG2(IEC_LOG_I, IO_TYPE_I, 'R', c->apdu_i.len - 4, c->apdu_i.ack, c->apdu_i.seq, connection_info(c));
            //if (m->seq != m->ack + 1) return -1;
            c->ack++;
            iec_asdu_t* obj = (iec_asdu_t*)&buf;
            if ((i = parse_asdu(&oi, obj, &count, (uint8_t*)&c->apdu_i.asdu, c->apdu_i.len - 4)) < 0) {
                c->e_in++;
                switch (i) {
                case -1:
                    LOG(IEC_LOG_ERROR, ERR_PARSING_ASDU, connection_info(c));
                    break;
                case -2:
                    LOG(IEC_LOG_WARNING, ERR_PARSING_ASDU_TYPE, oi.type, oi.ca, count, connection_info(c));
                    break;
                }
            }
            else {
                c->n_in++;
                c->n_in_i++;
                if (c->n_in_i == s->is.w) {
                    iec_send_s(c);
                    c->n_in_i = 0;
                }
                if (oi.ca != 0 && oi.ca != CA_BROADCAST(s)) {
                    for (i = 0; i < s->sector_count && s->sectors[i]->ca != oi.ca; i++);
                    if (i >= s->sector_count) {
                        LOG(IEC_LOG_WARNING, ERR_ASDU_INVALID_CA, get_ctx_name(oi.cause), oi.type, oi.ca, count, connection_info(c));
                        sector = NULL;
                    }
                    else {
                        LOG(IEC_LOG_ALL, IO_ASDU, 'R', get_ctx_name(oi.cause), oi.type, oi.ca, count, connection_info(c));
                        sector = s->sectors[i];
                        if (sector->port < 0) sector->port = s->port;
                    }
                }
                for (i = 0; i < count; i++) {
                    oi.obj = obj;
                    data = NULL;
                    int res = iec_sector_update_value(sector, &oi, &data);
                    if (res < 0)
                        switch (res) {
                        case ASDU_INVALID_CA:
                            LOG(IEC_LOG_WARNING, ERR_ASDU_INVALID_CA, get_ctx_name(oi.cause), oi.type, oi.ca, oi.obj->ioa, connection_info(c));
                            break;
                        case ASDU_INVALID_IOA:
                            LOG(IEC_LOG_WARNING, ERR_ASDU_INVALID_IOA, get_ctx_name(oi.cause), oi.type, oi.ca, oi.obj->ioa, connection_info(c));
                            break;
                        case ASDU_INVALID_TYPE:
                            LOG(IEC_LOG_WARNING, ERR_ASDU_INVALID_TYPE, get_ctx_name(oi.cause), oi.type, oi.ca, oi.obj->ioa, connection_info(c));
                            break;
                        case ASDU_TYPE_MISMATCH:
                            LOG(IEC_LOG_WARNING, ERR_ASDU_TYPE_MISMATCH, get_ctx_name(oi.cause), oi.type, oi.ca, oi.obj->ioa, connection_info(c));
                            break;
                        }
                    if (icb.iec_object_listener != NULL && data != NULL) {
                        info_object_t info;
                        export_io(data, &info);
                        icb.iec_object_listener(oi.ca, oi.obj->ioa, &info);
                    }
                    obj = (iec_asdu_t*)((intptr_t)obj + asdu_sizes[oi.type] + 4);
                }
            }
        }
        c->len -= c->apdu_u.len + 2;
        if (c->len == 0) return 1;
        memmove(c->buffer, c->buffer + c->apdu_u.len + 2, c->len);
    }
    return 0;
}

/*
 * Reading and parsing input data by SLAVE
 */
static int slave_read(iec_connection_t* c)
{
    int i, count;
    char buf[512];
    object_unit_t oi;
    iec_sector_t* sector;
    iec_socket_t* s = c->parent;
    icb.c_ca_len = s->is.ca_len;
    icb.c_ioa_len = s->is.ioa_len;
    icb.c_oa = s->is.oa;
    if (c->len == 0) return 0;
    while (1) {
        if (c->apdu_u.start != APDU_START) {
            c->e_in++;
            LOG(IEC_LOG_ERROR, ERR_START_BYTE, connection_info(c));
            return -1;
        }
        if (c->apdu_u.len > c->len - 2)
            return 1;
        hexdump(c->buffer, c->apdu_u.len + 2, 1);
        if (c->apdu_u.cb & 1) {
            if (c->apdu_u.len != 4) {
                c->e_in++;
                LOG(IEC_LOG_ALL, IO_TYPE_U, 'R', UNKNOWN, connection_info(c));
                return -1;
            }
            else if (c->apdu_u.cb & 2) {
                // U-frame
                LOG(IEC_LOG_ALL, IO_TYPE_U, 'R', get_u_name(c->apdu_u.cb & 0xfc), connection_info(c));
                LOG2(IEC_LOG_U, IO_TYPE_U, 'R', get_u_name(c->apdu_u.cb & 0xfc), connection_info(c));
                switch (c->apdu_u.cb & 0xfc) {
                case STARTDT_ACT:
                    c->n_in++;
                    iec_send_u(c, STARTDT_CON);
                    c->started = 1;
                    if ((s->flags & _IECF_S_IC_REQ) == 0)
                        process_slave_ic(s, c);
                    break;
                case TESTFR_ACT:
                    c->n_in++;
                    iec_send_u(c, TESTFR_CON);
                    break;
                case STOPDT_ACT:
                    c->n_in++;
                    iec_send_u(c, STOPDT_CON);
                    c->started = 0;
                    return -1;
                    break;
                case TESTFR_CON:
                    c->time_t3 = c->time_read;
                    c->time_t1 = 0;
                    break;
                case STOPDT_CON:
                    c->time_t3 = c->time_read;
                    c->time_t1 = 0;
                default:
                    LOG(IEC_LOG_ERROR, ERR_INVALID_CMD, c->apdu_u.cb & 0xfc, connection_info(c));
                    c->e_in++;
                    break;
                }
            }
            else {
                // S-frame
                if (check_sequense_s(c) < 0) return -1;
                c->time_t1 = 0;
                c->n_in++;
                c->apdu_s.ack >>= 1;
                c->n_out_i = 0;
                LOG(IEC_LOG_ALL, IO_TYPE_S, 'R', c->apdu_i.ack, connection_info(c));
                LOG2(IEC_LOG_S, IO_TYPE_S, 'R', c->apdu_i.ack, connection_info(c));
            }
        }
        else {
            // I-frame
            if (check_sequense_i(c) < 0) return -1;
            c->time_t1 = 0;
            c->time_t3 = c->time_read;
            c->apdu_i.seq >>= 1;
            c->apdu_i.ack >>= 1;
            LOG(IEC_LOG_ALL, IO_TYPE_I, 'R', c->apdu_i.len - 4, c->apdu_i.ack, c->apdu_i.seq, connection_info(c));
            LOG2(IEC_LOG_I, IO_TYPE_I, 'R', c->apdu_i.len - 4, c->apdu_i.ack, c->apdu_i.seq, connection_info(c));
            c->ack++;
            iec_asdu_t* obj = (iec_asdu_t*)&buf;
            if ((i = parse_asdu(&oi, obj, &count, (uint8_t*)&c->apdu_i.asdu, c->apdu_i.len - 4)) < 0) {
                c->e_in++;
                switch (i) {
                case -1:
                    LOG(IEC_LOG_ERROR, ERR_PARSING_ASDU, connection_info(c));
                    break;
                case -2:
                    LOG(IEC_LOG_WARNING, ERR_PARSING_ASDU_TYPE, oi.type, oi.ca, count, connection_info(c));
                    break;
                }
            }
            else {
                c->n_in++;
                c->n_in_i++;
                if (c->n_in_i == s->is.w) {
                    iec_send_s(c);
                    c->n_in_i = 0;
                }
                if (count > 1
                    || oi.cause != IEC_CT_ACT
                    /*                        || (oi.type & 0x80) == 0*/
                    || asdu_sizes[oi.type & 0x7f] == 255) {
                    LOG(IEC_LOG_WARNING, ERR_ASDU_INVALID_CMD, get_ctx_name(oi.cause), oi.type, oi.ca, count, connection_info(c));
                }
                else {
                    LOG(IEC_LOG_ALL, IO_ASDU, 'R', get_ctx_name(oi.cause), oi.type, oi.ca, count, connection_info(c));
                    sector = iec_socket_get_sector(s, oi.ca);
                    if (sector != NULL) {
                        if (obj->ioa > sector->max_ioa) {
                            LOG(IEC_LOG_WARNING, ERR_ASDU_INVALID_IOA, get_ctx_name(oi.cause), oi.type, oi.ca, obj->ioa, connection_info(c));
                        }
                        else if (c->started) {
                            oi.obj = obj;
                            iec_command(s, sector, &oi, c);
                        }
                    }
                    else {
                        if (oi.ca == 0) {
                            if (iec_command0(s, &oi, c) < 0) {
                                LOG(IEC_LOG_WARNING, ERR_ASDU_INVALID_CA, get_ctx_name(oi.cause), oi.type, oi.ca, count, connection_info(c));
                            }
                        }
                        else {
                            LOG(IEC_LOG_WARNING, ERR_ASDU_INVALID_CA, get_ctx_name(oi.cause), oi.type, oi.ca, count, connection_info(c));
                        }
                    }
                }
            }
        }
        c->len -= c->apdu_u.len + 2;
        if (c->len == 0) return 1;
        memmove(c->buffer, c->buffer + c->apdu_u.len + 2, c->len);
    }
    return 0;
}

/*
 * Return IEC synchronization object
 */
syncobj_t* iec_get_syncobj()
{
    return &icb.iec_sync;
}

void iec_set_event_listener(iec_event_listener_t listener)
{
    icb.iec_event_listener = listener;
}

void iec_set_object_listener(iec_object_listener_t listener)
{
    icb.iec_object_listener = listener;
}

void iec_set_status_listener(iec_status_listener_t listener)
{
    icb.iec_status_listener = listener;
}

const char* iec_lasterror(void)
{
    return icb.errormsg[0] ? icb.errormsg : iec_strerror(errno);
}

static struct {
    int code;
    const char* name;
} error_names[] = {
    {EI_TOOBIG, "value too big"},
    {EI_LIMIT, "limit exceeded"},
    {EI_SOCKET, "invalid socket"},
    {EI_SECTOR, "invalid common address of ASDU"},
    {EI_IOA, "invalid IOA"},
    {EI_TYPE, "invalid type of ASDU"},
    {EI_SOCKETEXIST, "socket already exists"},
    {EI_SECTOREXIST, "sector already exists"},
    {EI_IOAEXIST, "value already exists"},
    {EI_MASTEREXIST, "acquisition master already exists"},
    {EI_SYNTAX, "syntax error"},
    {EI_OPTIONS, "invalid option"},
    {0, "unknown error %d"},
};

const char* iec_strerror(int errcode)
{
    static char buffer[32] = {0};

    if (errcode >= 0)
    {
        return strerror(errcode);
    }

    int i = 0;
    for (i = 0; error_names[i].code != 0; i++)
    {
        if (error_names[i].code == errcode)
        {
            return error_names[i].name;
        }
    }

    sprintf(buffer, error_names[i].name, errcode);

    return buffer;
}

int iec_get_sockets_count()
{
    return icb.socket_count;
}

int iec_get_sectors_count()
{
    int i, j;
    for (i = j = 0; i < icb.sector_max; i++)
    {
        if (icb.sectors[i] != NULL)
        {
            j++;
        }
    }

    return j;
}

int iec_get_objects_count(uint32_t ca)
{
    iec_sector_t* sector;
    if ((sector = IEC_SECTOR(ca)) == NULL)
    {
        errno = EINVAL;
        return -1;
    }

    return sector->count;
}

int iec_get_sectors_list(int index, uint16_t** result)
{
    if (index >= icb.socket_count || result == NULL)
    {
        errno = EINVAL;
        return -1;
    }
    if (index >= 0) {
        iec_socket_t* s = IEC_SOCKET(index);
        int i, c = s->sector_count;
        if (c == 0) return 0;
        uint16_t* buf = (uint16_t*)malloc(c * 2);
        if (buf == NULL) return -1;
        for (i = 0; i < c; i++)
            buf[i] = s->sectors[i]->ca;
        *result = buf;
        return c;
    }
    else {
        int i, j, c = iec_get_sectors_count();
        if (c == 0) return 0;
        uint16_t* buf = (uint16_t*)malloc(c * 2);
        if (buf == NULL) return -1;
        for (i = j = 0; i < icb.sector_max; i++)
            if (icb.sectors[i] != NULL)
                buf[j++] = i + 1;
        *result = buf;
        return j;
    }
}

int iec_get_objects_list(uint32_t ca, uint32_t** result)
{
    iec_sector_t* sector;
    if (ca == 0 || ca > icb.sector_max || (sector = icb.sectors[ca]) == NULL)
    {
        errno = EINVAL;
        return -1;
    }
    int i, c = sector->count;
    if (c == 0) return 0;
    *result = (uint32_t*)malloc(c * 4);
    if (*result == NULL) return -1;
    for (i = 0; i < c; i++)
        *result[i] = sector->data[i].ioa;
    return c;
}

int iec_get_socket_info(int index, iec_socket_info_t * result)
{
    iec_socket_t* s = IEC_SOCKET(index);
    if ((s = IEC_SOCKET(index)) == NULL || result == NULL) {
        errno = EINVAL;
        return -1;
    }
    memset(result, 0, sizeof(iec_socket_info_t));
    if (s->flags & _IECF_S_CLIENT) result->flags |= (s->flags & _IECF_S_CLIENT) ? IECF_S_CLIENT : IECF_S_SERVER;
    if (s->flags & _IECF_S_MASTER) result->flags |= (s->flags & _IECF_S_MASTER) ? IECF_S_MASTER : IECF_S_SLAVE;
    if (s->flags & _IECF_S_IPV6) result->flags |= IECF_S_IPV6;
#ifdef __HAVE_SSL_SUPPORT__
    if (s->flags & _IECF_S_SSL) result->flags |= IECF_S_SSL;
#endif
    result->port = s->port;
    strncpy(result->address, s->addr, sizeof(result->address));
    if (s->flags & _IECF_S_CLIENT) {
        connection_t con = client_get_connection(s->client.client);
        result->received = connection_received(con);
        result->transmitted = connection_transmitted(con);
        return client_connected(s->client.client);
    }
    else {
        int i, c = server_get_count(s->server.server);
        for (i = 0; i < c; i++) {
            connection_t con = server_get_connection(s->server.server, i);
            result->received += connection_received(con);
            result->transmitted += connection_transmitted(con);
        }
        return c;
    }
}

/*
 * Create new sector structure
 * Params:
 * ca:          Common address of ASDU
 * max_count:   maximum values number
 * max_ioa:     maximum IOA value
 *
 * Return sector address if success, otherwise -1
 */
int iec_sector_new(const int ca, const int max_count, const int max_ioa)
{
    int size, ssize, isize;
    iec_sector_t* sector;
    if (max_count <= 0 || max_ioa <= 0 || max_ioa < max_count || ca < 1 || ca > icb.sector_max)
    {
        errno = EINVAL;
        SET_ERROR(ERR_SECTOR_NEW, errno, ca, strerror(errno));
        return -1;
    }

    if (icb.sectors[ca - 1] != NULL)
    {
        errno = EI_SECTOREXIST;
        SET_ERROR(ERR_SECTOR_NEW, errno, ca, iec_strerror(errno));
        return -1;
    }

    size = sizeof(struct iec_sector) + max_count * sizeof(iec_data_t);
    ssize = icb.socket_max * sizeof(int);
    isize = max_ioa <= MAX_INDEXED_IOA ? max_ioa * sizeof(int) : 0;
    sector = (iec_sector_t*)malloc(size + ssize + isize);
    if (sector == NULL)
    {
        SET_ERROR(ERR_SECTOR_NEW, errno, ca, strerror(errno));
        return -1;
    }

    memset(sector, 0, size + ssize);
    sector->slaves = (int*)((intptr_t)sector + size);
    if (isize)
    {
        sector->indexes = (int*)((intptr_t)sector + size + ssize);
        memset(sector->indexes, 0xff, isize);
    }

    sector->ca = ca;
    sector->max_count = max_count;
    sector->max_ioa = max_ioa;
    sector->master = -1;
    sector->port = -1;
    icb.sectors[ca - 1] = sector;
    return 0;
}

/*
 * Add new value to selected sector
 * Params:
 * ca:      Common address of ASDU
 * ioa:     Information Object Address
 * type:    Value type
 * flags:   Bitwise OR of following flags:
 *          IECF_O_NO_TIME:     use ASDU object without time
 *          IECF_O_NO_SPONT:    do not transmit on value change
 *          IECF_O_SUBSTITUTED: manual input
 *
 * Return 0 if success, otherwise -1
 */

int iec_sector_add_info(const int ca, uint32_t ioa, uint8_t type, uint8_t flags)
{
    iec_data_t* data;
    iec_sector_t* s = IEC_SECTOR(ca);
    errno = 0;
    if (s == NULL) errno = EI_SECTOR;
    else if (s->count == s->max_count) errno = EI_LIMIT;
    else if (ioa == 0 || ioa > s->max_ioa) errno = EI_IOA;
    else if (type >= sizeof(asdu_type_sizes)) errno = EI_TYPE;
    else data = iec_sector_data_new(s, ioa);
    if (errno != 0)
    {
        SET_ERROR(ERR_VALUE_ADD, GET_TYPE_NAME(type), ca, ioa, iec_strerror(errno));
        return -1;
    }

    memset(data, 0, sizeof(iec_data_t));
    data->type = type;
    data->flags = flags & (IECF_O_NO_SPONT | IECF_O_SUBSTITUTED);
    if ((flags & IECF_O_NO_TIME) == 0) data->flags |= _IECF_WITH_TIME;
    data->qds = (flags & IECF_O_SUBSTITUTED) ? (IECF_SB | IECF_BL | IECF_IV) : IECF_IV;
    data->ioa = ioa;
    return 0;
}

/*
 * Add new command to selected sector
 * Params:
 * ca:      Common address of ASDU
 * ioa:     Information Object Address
 * type:    Value type
 * flags:   Bitwise OR of the following flags:
 *          IECF_O_NO_TIME: use ASDU object without time
 * l_ca:    Common address of ASDU of controlled sector
 * l_ioa:   IOA of controlled value
 *
 * Return 0 if success, otherwise -1
 */
int iec_sector_add_cmd(const int ca, uint32_t ioa, uint8_t flags, const int l_ca, uint32_t l_ioa)
{
    iec_sector_t* sector, * l_sector;
    iec_data_t* data;
    iec_data_t* l_data;
    int type;
    errno = 0;
    sector = IEC_SECTOR(ca);
    l_sector = IEC_SECTOR(l_ca);
    if (sector == NULL || l_sector == NULL) errno = EI_SECTOR;
    else if (sector->count == sector->max_count) errno = EI_LIMIT;
    else if (ioa > sector->max_ioa || l_ioa > l_sector->max_ioa
        || (l_data = iec_sector_data(l_sector, l_ioa)) == NULL) errno = EI_IOA;
    else if ((type = l_data->type) >= sizeof(asdu_type_sizes)) errno = EI_TYPE;
    else data = iec_sector_data_new(sector, ioa);
    if (errno)
    {
        SET_ERROR(ERR_CMD_ADD, GET_TYPE_NAME(type), ca, ioa, l_ca, l_ioa, iec_strerror(errno));
        return -1;
    }
    memset(data, 0, sizeof(iec_data_t));
    data->type = type | 0x80;
    data->flags = (flags & IECF_O_NO_TIME) ? 0 : _IECF_WITH_TIME;
    data->ioa = ioa;
    data->link.ca = l_ca;
    data->link.ioa = l_ioa;
    return 0;
}

/*
 * Duplicate value (destination value replicates source)
 * Params:
 * ca:      Common address of ASDU
 * ioa:     Information Object Address
 * l_ca:    Common address of ASDU of destination sector
 * l_ioa:   IOA of destination value
 *
 * Return 0 if success, otherwise -1
 */
int iec_object_duplicate(const int ca, uint32_t ioa, const int l_ca, uint32_t l_ioa) {
    iec_sector_t* sector, * l_sector;
    iec_data_t* data;
    iec_data_t* l_data;
    errno = 0;
    sector = IEC_SECTOR(ca);
    l_sector = IEC_SECTOR(l_ca);
    if (sector == NULL || l_sector == NULL) errno = EI_SECTOR;
    else if (ioa > sector->max_ioa || l_ioa > l_sector->max_ioa) errno = EI_IOA;
    else if ((data = iec_sector_data(sector, ioa)) == NULL);
    else if ((data->type & 0x80) != 0) errno = EI_TYPE;
    else if (data->link.ca) errno = EI_IOAEXIST;
    else if (l_sector->count == l_sector->max_count) errno = EI_LIMIT;
    else l_data = iec_sector_data_new(l_sector, l_ioa);
    if (errno)
    {
        SET_ERROR(ERR_VALUE_DUP, ca, ioa, l_ca, l_ioa, iec_strerror(errno));
        return -1;
    }
    // if IOA not indexed, it is possible that address of data(ca, ioa) changed
    if (sector->indexes == NULL)
        data = iec_sector_data(sector, ioa);
    *l_data = *data;
    data->link.ca = l_ca;
    data->link.ioa = l_ioa;
    l_data->ioa = l_ioa;
    l_data->flags &= ~IECF_O_NO_SPONT;
    return 0;
}

/*
 * Set information object value and quality descriptor
 * Params:
 * ca:          Common address of ASDU
 * ioa:         Information Object Address
 * value:       Ptr to new data value (if NULL no changes)
 * qds:         Ptr to new quality descriptor (if NULL no changes)
 *              QDS can be combined with flags:
 *              IECF_QDS_COPY:  copy QDS
 *              IECF_QDS_SET:   set bits that set in new QDS (bitwise OR)
 *              IECF_QDS_RESET: clear bits that set in new QDS (bitwise AND NOT)
 * tv:          Ptr to <struct timeval> (if NULL use current time)
 *
 * Return 0 if success, otherwise -1
 */
int iec_object_set_value(const int ca, uint32_t ioa, void* value, uint16_t* qds, struct timeval* tv)
{
    struct timeval t;
    iec_data_t* data;
    iec_sector_t* s = IEC_SECTOR(ca);
    if (s == NULL
        || (value == NULL && qds == NULL)
        || ioa - 1 >= s->max_ioa
        || (data = iec_sector_data(s, ioa)) == NULL
        || data->type == 0) {
        errno = EINVAL;
        return -1;
    }
    if (tv == NULL)
        gettimeofday(&t, NULL);
    else
        t = *tv;
    s->tv = t;
    return iec_object_set_value_int(s, data, value, qds, &t);
}

/*
 * Send command confirmation frame
 * Params:
 * c:           Connection structure
 * sector:      Sector structure
 * data:        Data structure
 * ioa:         IOA
 * cmd:         Ptr to command
 *
 * Return 0 if success, otherwise -1
 */
static int iec_socket_confirm_cmd(iec_connection_t* c, iec_sector_t* sector, iec_data_t* data, uint32_t ioa, iec_variant_t* cmd) {
    int len = 0;
    uint8_t buf[256];
    create_asdu_func2_t f;
    uint8_t with_time = data->flags & _IECF_WITH_TIME;
    int type = data->type;
    f = create_asdu_func2[with_time][type & 0x7f];
    create_asdu_header(buf, &len, asdu_types2[with_time][type & 0x7f], 1, 0, IEC_CT_ACTCON, 0, 0, sector->ca);
    f(buf + len, &len, ioa, cmd);
    return iec_send_i(c, buf, len);
}

/*
 * Send IEC command
 * Params:
 * ca:          Common address of ASDU
 * ioa:         IOA
 * cmd:         Ptr to command
 * Return 0 if success, otherwise -1
 */
int iec_object_send_cmd(const int ca, uint32_t ioa, iec_variant_t* cmd)
{
    int len = 0;
    uint8_t buf[256];
    create_asdu_func2_t f;
    uint8_t with_time, type;
    iec_data_t* data;
    iec_sector_t* sector = IEC_SECTOR(ca);
    iec_socket_t* s = sector != NULL && sector->master >= 0 ? IEC_SOCKET(sector->master) : NULL;
    if (s == NULL
        || (s->flags & _IECF_S_MASTER) == 0
        || sector == NULL
        || ioa - 1 >= sector->max_ioa
        || (data = iec_sector_data(sector, ioa)) == NULL
        || (((type = data->type) & 0x80) == 0)
        || (f = create_asdu_func2[with_time = data->flags & _IECF_WITH_TIME][type & 0x7f]) == NULL) {
        errno = EINVAL;
        return -1;
    }
    create_asdu_header(buf, &len, asdu_types2[with_time][type & 0x7f], 1, 0, IEC_CT_ACT, 0, 0, sector->ca);
    f(buf + len, &len, ioa, cmd);
    return iec_send_i(&s->client.connection, buf, len);
}

/*
 * Fill structure with object information by common address of ASDU and IOA
 * Return 0 if success, otherwise -1
 */
int iec_object_get_info(const int ca, uint32_t ioa, info_object_t* info)
{
    iec_data_t* data;
    if (info == NULL || (data = iec_object_get_data(ca, ioa)) == NULL)
    {
        errno = EINVAL;
        return -1;
    }
    export_io(data, info);
    return 0;
}

/*
 * Return ptr to user data by common address of ASDU and IOA
 */
void* iec_object_get_userdata(const int ca, uint32_t ioa)
{
    iec_data_t* data;
    data = iec_object_get_data(ca, ioa);
    return data == NULL ? NULL : data->userdata;
}

/*
 * Set ptr to user data by common address of ASDU and IOA
 */
void iec_object_set_userdata(const int ca, uint32_t ioa, void* userdata)
{
    iec_data_t* data;
    data = iec_object_get_data(ca, ioa);
    if (data != NULL) data->userdata = userdata;
}

/*
 * Update data with just received value
 *
 * Return:      0 if success, otherwise error code:
 *              -1: invalid ASDU CA
 *              -2: invalid IOA
 *              -3: invalid ASDU type
 *              -4: type mismatch
 */
static int iec_sector_update_value(iec_sector_t* sector, object_unit_t* info, iec_data_t** dataptr)
{
    iec_asdu_t* obj = info->obj;
    uint8_t type = info->type;
    uint32_t ioa = obj->ioa;
    iec_data_t* data;
    static iec_data_t static_data;
    *dataptr = NULL;
    if (ioa == 0) switch (type) {
    case C_IC_NA_1:
        return 0;
    default:
        return ASDU_INVALID_IOA;
    }
    int result = 0;
    int found = sector != NULL;
    if (!found) result = ASDU_INVALID_CA;
    if (!found || (ioa < 1 || ioa > sector->max_ioa || (data = iec_sector_data(sector, ioa)) == NULL)) {
        if (!result) result = ASDU_INVALID_IOA;
        data = &static_data;
        data->type = asdu_value_types[type];
        data->ioa = ioa;
        data->qds = 0;
        found = 0;
    }
    if (asdu_value_types[type] == 0) {
        result = ASDU_INVALID_TYPE;
    }
    if (asdu_value_types[type] != data->type) {
        result = ASDU_TYPE_MISMATCH;
        data = &static_data;
        data->type = asdu_value_types[type];
        data->ioa = ioa;
        data->qds = 0;
        found = 0;
    }
    if ((data->qds & (IECF_BL | IECF_SB)) != 0) return result;
    struct timeval tv;
    gettimeofday(&tv, NULL);
    data->cause = info->cause;

#define NOT_MODIFIED(q, v1, v2) ((data->qds & ~IECF_NT) == q && v1 == v2 && data->tv.tv_sec == tv.tv_sec && data->tv.tv_usec == tv.tv_usec)
    switch (type)
    {
    case M_SP_TB_1:
        cp56time2a_to_tv(&obj->type30.time, &tv);
    case M_SP_NA_1: // Single point
        if NOT_MODIFIED(obj->type1.qds, data->value.uint8, (obj->type1.qds & IECF_SPI)) break;
        data->tv = tv;
        data->qds = obj->type1.qds;
        data->value.uint8 = obj->type1.qds & IECF_SPI;
        if (found) iec_sector_enqueue_data(sector, ioa, &data->tv, IEC_CT_SPONT);
        break;
    case M_DP_TB_1:
        cp56time2a_to_tv(&obj->type31.time, &tv);
    case M_DP_NA_1: // Double point
        if NOT_MODIFIED(obj->type3.qds, data->value.uint8, (obj->type3.qds & IECF_DPI)) break;
        data->tv = tv;
        data->qds = obj->type3.qds;
        data->value.uint8 = obj->type3.qds & IECF_DPI;
        if (found) iec_sector_enqueue_data(sector, ioa, &data->tv, IEC_CT_SPONT);
        break;
    case M_ST_TB_1:
        cp56time2a_to_tv(&obj->type32.time, &tv);
    case M_ST_NA_1: // Step value
        if NOT_MODIFIED(obj->type5.qds, data->value.uint8, (obj->type5.vti & IECF_STEP)) break;
        data->tv = tv;
        data->qds = obj->type5.qds;
        data->value.uint8 = obj->type5.vti & IECF_STEP;
        if (found) iec_sector_enqueue_data(sector, ioa, &data->tv, IEC_CT_SPONT);
        break;
    case M_BO_TB_1:
        cp56time2a_to_tv(&obj->type33.time, &tv);
    case M_BO_NA_1: // Bitstring
        if NOT_MODIFIED(obj->type7.qds, data->value.uint32, obj->type7.stcd) break;
        data->tv = tv;
        data->qds = obj->type7.qds;
        data->value.uint32 = obj->type7.stcd;
        if (found) iec_sector_enqueue_data(sector, ioa, &data->tv, IEC_CT_SPONT);
        break;
    case M_ME_TD_1:
        cp56time2a_to_tv(&obj->type34.time, &tv);
    case M_ME_NA_1: // Normalized value
        if NOT_MODIFIED(obj->type9.qds, data->value.uint16, obj->type9.mv) break;
        data->tv = tv;
        data->qds = obj->type9.qds;
        data->value.uint16 = obj->type9.mv;
        if (found) iec_sector_enqueue_data(sector, ioa, &data->tv, IEC_CT_SPONT);
        break;
    case M_ME_TE_1:
        cp56time2a_to_tv(&obj->type35.time, &tv);
    case M_ME_NB_1: // Scaled value
        if NOT_MODIFIED(obj->type11.qds, data->value.int16, obj->type11.mv) break;
        data->tv = tv;
        data->qds = obj->type11.qds;
        data->value.int16 = obj->type11.mv;
        if (found) iec_sector_enqueue_data(sector, ioa, &data->tv, IEC_CT_SPONT);
        break;
    case M_ME_TF_1:
        cp56time2a_to_tv(&obj->type36.time, &tv);
    case M_ME_NC_1: // Float value
        if NOT_MODIFIED(obj->type13.qds, data->value.real, obj->type13.mv) break;
        data->tv = tv;
        data->qds = obj->type13.qds;
        data->value.real = obj->type13.mv;
        if (found) iec_sector_enqueue_data(sector, ioa, &data->tv, IEC_CT_SPONT);
        break;
    case M_IT_TB_1:
        cp56time2a_to_tv(&obj->type37.time, &tv);
    case M_IT_NA_1: // Integrated value
        if NOT_MODIFIED(obj->type15.qds, data->value.uint32, obj->type15.bcr) break;
        data->tv = tv;
        data->qds = obj->type15.qds;
        data->value.uint32 = obj->type15.bcr;
        if (found) iec_sector_enqueue_data(sector, ioa, &data->tv, IEC_CT_SPONT);
        break;
    default:
        return ASDU_INVALID_TYPE;
    }
#undef NOT_MODIFIED
    * dataptr = data;
    if (found) {
        sector->tv = data->tv;
        sector->received++;
        if ((type & 0x80) == 0 && data->link.ca != 0) {
            uint16_t qds = data->qds;
            iec_object_set_value(data->link.ca, data->link.ioa, &data->value, &qds, &data->tv);
        }
    }
    return result;
}

/*
 * Send all sector data
 */
static int iec_slave_enqueue_sector(iec_connection_t* c, iec_sector_t* sector)
{
    int i;
    queue_item_t q;
    q.ca = sector->ca;
    // Confirm activation
    iec_send_i_100(c, sector->ca, IEC_CT_ACTCON);
    for (i = 0; i < sector->count; i++) if ((sector->data[i].type & 0x80) == 0)
    {
        q.data = sector->data[i];
        q.data.cause = IEC_CT_INTROGEN;
        iec_enqueue_item(c, &q);
    }
    // Send data
    send_queue(c->parent, c);
    // End activation
    iec_send_i_100(c, sector->ca, IEC_CT_ACTTERM);
    return 0;
}

/*
 * Process IEC104 commands if CA = 0; not finished yet!
 */
static int iec_command0(iec_socket_t* s, object_unit_t* info, iec_connection_t* c)
{
    switch (info->type)
    {
    case C_IC_NA_1: /* 100 - Interrogation command */
        process_slave_ic(s, c);
        break;
    case C_CS_NA_1: /* 103 - Clock synchronization command */
        // Sync time here
        iec_send_i_103(c, 0);
        break;
    case C_TS_NA_1: /* 104 - Test command */
        iec_send_i_104(c, 0);
        break;
    case C_TS_TA_1: /* 107 - Test command with time tag CP56Time2a */
        iec_send_i_107(c, 0, info->obj->type107.tsc);
        break;
    case C_RP_NA_1: /* 105 - Reset process command */
        iec_send_i_105(c, 0);
        c->queue->size = 0;
        c->e_in = 0;
        c->n_in = 0;
        c->n_in_i = 0;
        break;
    default:
        return -1;
    }
    return 0;
}

/*
 * Process IEC104 commands; not finished
 */
static int iec_command(iec_socket_t* s, iec_sector_t* sector, object_unit_t* info, iec_connection_t* c)
{
    struct timeval tv;
    uint16_t ca = info->ca;
    iec_asdu_t* obj = info->obj;
    uint8_t type = info->type;
    uint32_t ioa = obj->ioa;
    iec_data_t* data = ioa > 0 ? iec_sector_data(sector, ioa) : NULL;
    if (info->cause != IEC_CT_ACT) { // ???
        LOG(IEC_LOG_WARNING, ERR_UNKNOWN_CAUSE, type, info->ca, ioa, info->cause, connection_info(c));
        return -1;
    }
    switch (type) {
    case C_IC_NA_1: /* 100 - Interrogation command */
        if (IS_CA_BROADCAST(s, ca)) {
            process_slave_ic(s, c);
        }
        else {
            if (sector != NULL)
                iec_slave_enqueue_sector(c, sector);
            else
                iec_send_i_100(c, ca, IEC_CT_UK_CA);
        }
        break;
    case C_CI_NA_1: /* 101 - Counter interrogation command */
        iec_send_i_101(c, ca, IEC_GET_FRZ(obj->type101.qcc));
        break;
    case C_RD_NA_1: /* 102 - Read command */
        iec_sector_enqueue_data1(c, sector, ioa);
        break;
    case C_CS_NA_1: /* 103 - Clock synchronization command */
        // Sync time here
        iec_send_i_103(c, ca);
        break;
    case C_TS_NA_1: /* 104 - Test command */
        iec_send_i_104(c, ca);
        break;
    case C_TS_TA_1: /* 107 - Test command with time tag CP56Time2a */
        iec_send_i_107(c, ca, info->obj->type107.tsc);
        break;
    case C_RP_NA_1: /* 105 - Reset process command */
        // Reset process here
        iec_send_i_105(c, ca);
        break;
    case C_SC_TA_1:
        cp56time2a_to_tv(&obj->type58.time, &tv);
    case C_SC_NA_1: // Single command
        if (data->type != IECC_SINGLE) {
            LOG(IEC_LOG_WARNING, ERR_COMMAND_TYPE, type, info->ca, ioa, connection_info(c));
            break;
        }
        if (obj->type45.sco & IECF_SE) {
            break;
        }
        data->cause = info->cause;
        obj->type45.sco &= IECF_SCS;
        if (icb.iec_cmd_func == NULL || icb.iec_cmd_func(ca, ioa, &data->value) == 0)
            iec_object_set_value(ca, ioa, &obj->type45.sco, NULL, type == C_SC_NA_1 ? NULL : &tv);
        iec_socket_confirm_cmd(c, sector, data, ioa, &data->value);
        break;
    case C_DC_TA_1:
        cp56time2a_to_tv(&obj->type59.time, &tv);
    case C_DC_NA_1: // Double command
        if (data->type != IECC_DOUBLE) {
            LOG(IEC_LOG_WARNING, ERR_COMMAND_TYPE, type, info->ca, ioa, connection_info(c));
            break;
        }
        if (obj->type46.dco & IECF_SE) {
            break;
        }
        data->cause = info->cause;
        obj->type46.dco &= IECF_DCS;
        if (icb.iec_cmd_func == NULL || icb.iec_cmd_func(ca, ioa, &data->value) == 0)
            iec_object_set_value(ca, ioa, &obj->type46.dco, NULL, type == C_DC_NA_1 ? NULL : &tv);
        iec_socket_confirm_cmd(c, sector, data, ioa, &data->value);
        break;
    case C_RC_TA_1:
        cp56time2a_to_tv(&obj->type60.time, &tv);
    case C_RC_NA_1: // Regulating step command
        if (data->type != IECC_STEP) {
            LOG(IEC_LOG_WARNING, ERR_COMMAND_TYPE, type, info->ca, ioa, connection_info(c));
            break;
        }
        if (obj->type47.rco & IECF_SE) {
            break;
        }
        data->cause = info->cause;
        obj->type47.rco &= IECF_RCS;
        if (icb.iec_cmd_func == NULL || icb.iec_cmd_func(ca, ioa, &data->value) == 0)
            iec_object_set_value(ca, ioa, &obj->type47.rco, NULL, type == C_RC_NA_1 ? NULL : &tv);
        iec_socket_confirm_cmd(c, sector, data, ioa, &data->value);
        break;
    case C_SE_TA_1:
        cp56time2a_to_tv(&obj->type61.time, &tv);
    case C_SE_NA_1: // Set-point command, normalized value
        if (data->type != IECC_NORMALIZED) {
            LOG(IEC_LOG_WARNING, ERR_COMMAND_TYPE, type, info->ca, ioa, connection_info(c));
            break;
        }
        if (obj->type48.qos & IECF_SE) {
            break;
        }
        data->cause = info->cause;
        if (icb.iec_cmd_func == NULL || icb.iec_cmd_func(ca, ioa, &data->value) == 0)
            iec_object_set_value(ca, ioa, &obj->type48.nva, NULL, type == C_SE_NA_1 ? NULL : &tv);
        iec_socket_confirm_cmd(c, sector, data, ioa, &data->value);
        break;
    case C_SE_TB_1:
        cp56time2a_to_tv(&obj->type62.time, &tv);
    case C_SE_NB_1: // Set-point command, scaled value
        if (data->type != IECC_SCALED) {
            LOG(IEC_LOG_WARNING, ERR_COMMAND_TYPE, type, info->ca, ioa, connection_info(c));
            break;
        }
        if (obj->type49.qos & IECF_SE) {
            break;
        }
        data->cause = info->cause;
        if (icb.iec_cmd_func == NULL || icb.iec_cmd_func(ca, ioa, &data->value) == 0)
            iec_object_set_value(ca, ioa, &obj->type49.sva, NULL, type == C_SE_NB_1 ? NULL : &tv);
        iec_socket_confirm_cmd(c, sector, data, ioa, &data->value);
        break;
    case C_SE_TC_1:
        cp56time2a_to_tv(&obj->type63.time, &tv);
    case C_SE_NC_1: // Set-point command, float value
        if (data->type != IECC_FLOAT) {
            LOG(IEC_LOG_WARNING, ERR_COMMAND_TYPE, type, info->ca, ioa, connection_info(c));
            break;
        }
        if (obj->type50.qos & IECF_SE) {
            break;
        }
        data->cause = info->cause;
        if (icb.iec_cmd_func == NULL || icb.iec_cmd_func(ca, ioa, &data->value) == 0)
            iec_object_set_value(ca, ioa, &obj->type50.r32, NULL, type == C_SE_NC_1 ? NULL : &tv);
        iec_socket_confirm_cmd(c, sector, data, ioa, &data->value);
        break;
    case C_BO_TA_1:
        cp56time2a_to_tv(&obj->type64.time, &tv);
    case C_BO_NA_1: // Bitstring of 32 bit command
        if (data->type != IECC_BITSTRING) {
            LOG(IEC_LOG_WARNING, ERR_COMMAND_TYPE, type, info->ca, ioa, connection_info(c));
            break;
        }
        data->cause = info->cause;
        if (icb.iec_cmd_func == NULL || icb.iec_cmd_func(ca, ioa, &data->value) == 0)
            iec_object_set_value(ca, ioa, &obj->type51.bsi, NULL, type == C_BO_NA_1 ? NULL : &tv);
        iec_socket_confirm_cmd(c, sector, data, ioa, &data->value);
        break;
    }
    return 0;
}

/*
 * Return sector status
 * Params:
 * ca:          Common address of ASDU
 *
 * Return:      1 if corresponding socket is MASTER and it is connected
 *              0 if corresponding socket is MASTER and it is disconnected
 *              -1 if invalid CA or corresponding socket is SLAVE
 */
int iec_sector_status(const int ca)
{
    iec_sector_t* sector;
    if ((sector = IEC_SECTOR(ca)) == NULL || sector->master < 0) return -1;
    if (sector->port < 0) return 0;
    return iec_socket_status(sector->master);
}

/* Return internal representation of flags if success, otherwise -1 */
static int check_socket_flags(int flags)
{
    int f;
    if (flags == 0) return -1;
    f = flags & (IECF_S_CLIENT | IECF_S_SERVER);
    if (f == 0 || f == (IECF_S_CLIENT | IECF_S_SERVER)) return -1;
    f = flags & (IECF_S_MASTER | IECF_S_SLAVE);
    if (f == (IECF_S_MASTER | IECF_S_SLAVE)) return -1;
    if (f == 0) flags |= (flags & IECF_S_CLIENT) ? IECF_S_MASTER : IECF_S_SLAVE;
    if (flags & IECF_S_CLIENT) f |= _IECF_S_CLIENT;
    if (flags & IECF_S_MASTER) f |= _IECF_S_MASTER;
    if (flags & IECF_S_IC_REQ) f |= _IECF_S_IC_REQ;
#ifdef __HAVE_SSL_SUPPORT__
    if (flags & IECF_S_SSL) f |= _IECF_S_SSL;
#endif
    return f;
}

/*
 * Creates new IEC104 socket
 * Params:
 * addr:    connection address for client, bind address for server
 * port:    socket port
 * count:   max number of sectors
 * flags:
 *          IECF_S_IPV6 - use ipv6, otherwise ipv4
 *          IECF_S_CLIENT - client socket
 *          IECF_S_SERVER - server socket
 *          IECF_S_MASTER - master mode (default for client)
 *          IECF_S_SLAVE - slave mode (default for server)
 * Return IEC socket ID if success, otherwise -1
 */
int iec_socket_new(const char* addr, const int port, const int count, const int flags, iec_settings_t* is)
{
    int i, f, size, qsize;
    iec_socket_t* s;
    if (port < 1
        || port > 65535
        || count < 0
        || (f = check_socket_flags(flags)) < 0
        || ((f & _IECF_S_CLIENT) != 0 && addr == NULL))
    {
        errno = EINVAL;
        SET_ERROR(ERR_SOCKET_NEW, errno, addr == NULL ? "NULL" : addr, port, flags, strerror(errno));
        return -1;
    }

    if (icb.socket_count == icb.socket_max)
    {
        errno = EI_LIMIT;
        SET_ERROR(ERR_SOCKET_NEW, errno, addr, port, f, iec_strerror(errno));
        return -1;
    }

    if ((f & _IECF_S_CLIENT) == 0)
        for (i = 0; i < icb.socket_count; i++)
        {
            s = icb.sockets[i];
            if ((s->flags & _IECF_S_CLIENT) == 0 && s->port == port)
            {
                errno = EI_SOCKETEXIST;
                SET_ERROR(ERR_SOCKET_NEW, errno, addr, port, f, iec_strerror(errno));
                return -1;
            }
        }
    size = sizeof(struct iec_socket) + count * sizeof(iec_sector_t);
    int sf = (f & _IECF_S_IPV6) ? SF_IPV6 : 0;
#ifdef __HAVE_SSL_SUPPORT__
    sf |= (f & _IECF_S_SSL) ? SF_SSL : 0;
#endif
    if (f & _IECF_S_CLIENT)
    {
        qsize = (f & _IECF_S_MASTER) ? 0 : sizeof(struct iec_queue) + QUEUE_SIZE * sizeof(queue_item_t);
        s = (iec_socket_t*)calloc(1, size + qsize);
        if (s == NULL) {
            SET_ERROR(ERR_CLIENT_NEW, errno, addr, port, strerror(errno));
            return -1;
        }
        s->client.client = client_new(addr, port, sf, &icb.iec_sync);
        if (s->client.client == NULL) {
            free(s);
            return -1;
        }
        if (qsize) s->client.connection.queue = (iec_queue_t*)((intptr_t)s + size);
        s->client.connection.parent = s;
        strncpy(s->client.connection.addr, addr, sizeof(s->client.connection.addr));
        s->client.connection.port = port;
        client_set_data(s->client.client, s);
        client_set_listener(s->client.client, iec_client_listener);
    }
    else
    {
        s = (iec_socket_t*)calloc(1, size);
        if (s == NULL)
        {
            SET_ERROR(ERR_SERVER_NEW, errno, port, strerror(errno));
            return -1;
        }
        s->server.server = server_new(addr, port, sf, &icb.iec_sync);
        if (s->server.server == NULL)
        {
            SET_ERROR(ERR_SERVER_NEW, errno, port, strerror(errno));
            free(s);
            return -1;
        }
        server_set_data(s->server.server, s);
        server_set_listener(s->server.server, iec_server_listener);
    }
    if (addr != NULL) strncpy(s->addr, addr, sizeof(s->addr));
    s->port = port;
    s->flags = f;
    s->sector_max = count;
    s->is = is == NULL ? icb.is : *is;
    //if (s->is.k == 0) s->is.k = icb.is.k;
    if (s->is.w == 0) s->is.w = icb.is.w;
    if (s->is.t0 == 0) s->is.t0 = icb.is.t0;
    if (s->is.t1 == 0) s->is.t1 = icb.is.t1;
    if (s->is.t2 == 0) s->is.t2 = icb.is.t2;
    if (s->is.t3 == 0) s->is.t3 = icb.is.t3;
    if (s->is.ca_len == 0) s->is.ca_len = icb.is.ca_len;
    if (s->is.ioa_len == 0) s->is.ioa_len = icb.is.ioa_len;
    s->index = icb.socket_count;
    icb.sockets[icb.socket_count++] = s;
    return s->index;
}

/*
 * Add sector to selected socket
 * Params:
 * index:       Socket ID
 * ca:          Common address of ASDU
 *
 * Return 0 if success, otherwise -1
 */
int iec_socket_add_sector(const int index, const int ca)
{
    iec_sector_t* sector;
    iec_socket_t* s;
    if ((sector = IEC_SECTOR(ca)) == NULL)
    {
        errno = EI_SECTOR;
        SET_ERROR(ERR_SOCKET_ADD, ca, index, iec_strerror(errno));
        return -1;
    }

    if ((s = IEC_SOCKET(index)) == NULL)
    {
        errno = EI_SOCKET;
        SET_ERROR(ERR_SOCKET_ADD, ca, index, iec_strerror(errno));
        return -1;
    }
    if (s->sector_count == s->sector_max)
    {
        errno = EI_LIMIT;
        SET_ERROR(ERR_SOCKET_ADD, ca, index, iec_strerror(errno));
        return -1;
    }
    if (iec_socket_get_sector(s, ca) != NULL)
    {
        errno = EI_SECTOREXIST;
        SET_ERROR(ERR_SOCKET_ADD, ca, index, iec_strerror(errno));
        return -1;
    }
    if (s->flags & _IECF_S_MASTER)
    {
        if (sector->master >= 0) {
            errno = EI_MASTEREXIST;
            SET_ERROR(ERR_SOCKET_ADD, ca, index, iec_strerror(errno));
            return -1;
        }
        sector->master = index;
    }
    s->sectors[s->sector_count++] = sector;
    if ((s->flags & _IECF_S_MASTER) == 0)
    {
        sector->slaves[sector->n_slaves++] = index;
    }
    return 0;
}

/*
 * Clear output queue for selected socket ID
 */
int iec_server_clear_queue(const int index)
{
    int i, count;
    iec_socket_t* s = IEC_SOCKET(index);
    if (s == NULL || (s->flags & _IECF_S_MASTER))
    {
        errno = EINVAL;
        return -1;
    }
    if (s->flags & _IECF_S_CLIENT)
    {
        s->client.connection.queue->size = 0;
        return 0;
    }
    for (i = 0, count = server_get_count(s->server.server); i < count; i++)
    {
        iec_connection_t* c = iec_server_connection(s, i);
        c->queue->size = 0;
    }
    return 0;
}

/* Return 1 if selected socket is active (connected or listening), othewise 0 */
int iec_socket_status(const int index)
{
    iec_socket_t* s = IEC_SOCKET(index);
    if (s == NULL) return -1;
    if (s->flags & _IECF_S_CLIENT)
    {
        return client_connected(s->client.client);
    }
    else
    {
        return server_started(s->server.server);
    }
}

/* Set allowed ip addresses for server socket */
int iec_socket_set_allowed_list(const int index, const char* addr_list)
{
    int i, j, n, size;
    char* p;
    char addr[INET6_ADDRSTRLEN];
    struct in_addr* in4;
    struct in6_addr* in6;
    iec_socket_t* s = IEC_SOCKET(index);
    if (s == NULL || (s->flags & _IECF_S_CLIENT) != 0) {
        errno = EINVAL;
        SET_ERROR(ERR_ALLOW_IP_LIST, "NULL", s == NULL ? 0 : s->port, strerror(errno));
        return -1;
    }
    if (s->server.access_list != NULL)
    {
        free(s->server.access_list);
        s->server.access_list = NULL;
        s->server.access_count = 0;
    }

    if (addr_list == NULL || *addr_list == 0) return 0;
    for (n = 1, p = (char*)addr_list; *p != 0; p++) if (*p == ',') n++;
    s->server.access_list = calloc(n, (size = (s->flags & _IECF_S_IPV6) ? sizeof(struct in6_addr) : sizeof(struct in_addr)));
    if (s->server.access_list == NULL) return -1;
    if (s->flags & _IECF_S_IPV6)
    {
        in6 = (struct in6_addr*)(s->server.access_list);
    }
    else {
        in4 = (struct in_addr*)(s->server.access_list);
    }
    s->server.access_count = n;
    for (i = 0, p = (char*)addr_list; i < n; i++) {
        while (*p == ' ') p++;
        for (j = 0; *p != 0 && *p != ',' && j < sizeof(addr); j++, p++) addr[j] = *p;
        if (j == sizeof(addr)) goto error;
        addr[j] = 0;
        if (s->flags & _IECF_S_IPV6)
        {
            if (inet_pton(AF_INET6, addr, &in6[i]) <= 0) goto error;
        }
        else {
            if (inet_pton(AF_INET, addr, &in4[i]) <= 0) goto error;
        }
        if (*p == 0) break;
        p++;
    }
    return 0;
error:
    free(s->server.access_list);
    s->server.access_list = NULL;
    s->server.access_count = 0;
    if (errno == 0) errno = EINVAL;
    SET_ERROR(ERR_ALLOW_IP_LIST, addr, s->port, strerror(errno));
    return -1;
}

int iec_save_data(const char* filename)
{
    int file = open(filename, O_WRONLY | O_CREAT | O_TRUNC, 0644);
    if (file < 0) return -1;
    int i, j, n;
    store_data_t* dptr;
    iec_sector_t* sector;
    iec_data_t* data;
    store_data_t* datalist = (store_data_t*)malloc(STORE_DATA_NUM * sizeof(store_data_t));
    if (datalist == NULL) {
        close(file);
        return -1;
    }
    dptr = datalist;
    for (i = n = 0; i < icb.sector_max; i++)
        if ((sector = icb.sectors[i]) != NULL)
            for (j = 0; j < sector->count; j++) {
                data = &sector->data[j];
                dptr->ca = sector->ca;
                dptr->ioa = data->ioa;
                dptr->type = data->type;
                dptr->tv = data->tv;
                dptr->value = data->value;
                n++;
                if (n == STORE_DATA_NUM) {
                    write(file, datalist, sizeof(store_data_t) * n);
                    dptr = datalist;
                    n = 0;
                }
                else
                    dptr++;
            }
    if (n > 0)
        write(file, datalist, sizeof(store_data_t) * n);
    free(datalist);
    close(file);
    return 0;
}

int iec_load_data(const char* filename)
{
    int file = open(filename, O_RDONLY);
    if (file < 0) return -1;
    int i, n, res;
    //uint16_t qds;
    //struct timeval tv;
    store_data_t* dptr;
    iec_data_t* data;
    store_data_t* datalist = (store_data_t*)malloc(STORE_DATA_NUM * sizeof(store_data_t));
    if (datalist == NULL) {
        close(file);
        return -1;
    }
    while (1) {
        res = read(file, datalist, STORE_DATA_NUM * sizeof(store_data_t));
        if (res < 0 || (res % sizeof(store_data_t)) != 0) {
            free(datalist);
            close(file);
            return -1;
        }
        if (res == 0) break;
        n = res / sizeof(store_data_t);
        for (i = 0, dptr = datalist; i < n; i++, dptr++) {
            data = iec_object_get_data(dptr->ca, dptr->ioa);
            if (data != NULL && dptr->type == data->type) { // ???
                data->value = dptr->value;
                data->tv = dptr->tv;
            }
            else {
                free(datalist);
                close(file);
                SET_ERROR(ERR_INIT_DATA, EINVAL, filename);
                errno = EINVAL;
                return -1;
            }
        }
    }
    free(datalist);
    close(file);
    return 0;
}

int iec_set_status_sector(const int ca)
{
    if (ca < 0 || ca > IEC_MAX_SECTORS)
    {
        errno = EINVAL;
        return -1;
    }

    status_sector = ca;
    return 0;
}

/* Return IEC104 status: 1 - running, otherwise 0 */
int iec_running()
{
    return icb.flags & IECF_RUNNING;
}

/* Lock / unlock IEC processing */
int iec_lock()
{
    return sync_lock(&icb.iec_sync);
}

int iec_unlock()
{
    return sync_unlock(&icb.iec_sync);
}

void iec_set_command_func(iec_command_func_t func)
{
    icb.iec_cmd_func = func;
}

void iec_set_timer_func(iec_timer_func_t func)
{
    icb.iec_timer_func = func;
}

/*
 * Load all file contents into memory
 * Return allocated buffer ptr if success, otherwise NULL
 */
static char* load_file(const char* name)
{
    char* buf = NULL;
    int i = 0, file = 0, size = 0;
    struct stat stat;

    file = open(name, O_RDONLY);
    if (file < 0)
    {
        return NULL;
    }

    if (fstat(file, &stat) != 0)
    {
        goto error1;
    }

    size = stat.st_size;
    buf = (char*)malloc(size + 1);

    if (buf == NULL)
    {
        goto error1;
    }

    i = read(file, buf, size);
    if (i < 0)
    {
        goto error2;
    }

    if (i != size)
    {
        goto error2;
    }

    close(file);
    buf[size] = 0;

    return buf;

error2:
    free(buf);
error1:
    close(file);

    return NULL;
}

static int read_linked_addr(char** buf, int* ca, int* ioa)
{
    char* p = *buf;
    char* end;
    while (*p == ' ')
    {
        p++;
    }

    if (*p == '\t')
    {
        p++;
        *ca = 0;
    }
    else
    {
        *ca = strtol(p, &end, 10);
        if (*end != '\t' || *ca < 0 || *ca > IEC_MAX_SECTORS)
        {
            errno = EI_SYNTAX;
            return -1;
        }
        p = end + 1;
    }

    while (*p == ' ')
    {
        p++;
    }

    if (*p == '\t')
    {
        *ioa = 0;
        p++;
    }
    else
    {
        *ioa = strtol(p, &end, 10);
        if (*end != '\t' || *ioa < 0 || *ioa > IEC_MAX_IOA)
        {
            errno = EI_SYNTAX;
            return -1;
        }
        p = end + 1;
    }

    *buf = p;
    return *ca == 0 && *ioa == 0 ? 0 : 1;
}

static int load_sockets(char* buf, iec_settings_t* is)
{
    char* end;
    char* p;
    int i, j, line, off, index;
    socket_t* s;
    nsockets = 0;

    for (p = buf, line = 1, s = sockets; *p != 0; p++, line++)
    {
        s->is = *is;
        switch (*p)
        {
        case '\n':
        case '#':
            break;
        default:
            // 1: flag
            i = strtol(p, &end, 10);
            ASSERT(*end != '\t', EI_SYNTAX);
            ASSERT(i != 0 && i != 1, EINVAL);
            if (i == 0)
            {
                break;
            }
            p = end + 1;
            // 2: port
            s->port = strtol(p, &end, 10);
            ASSERT(*end != '\t', EI_SYNTAX);
            ASSERT(s->port <= 0 || s->port > 65535, EINVAL);
            ASSERT(nsockets == IEC_MAX_SOCKETS, EI_LIMIT);
            p = end + 1;
            // 3: IP address (for client socket)
            for (; *p == ' '; p++);
            for (end = p, i = 0; i < INET6_ADDRSTRLEN && *p != 0 && *p != '\t' && *p != '\n'; i++, p++);
            ASSERT(*p != '\t', EI_SYNTAX);
            memmove(&s->ip, end, i);
            s->ip[i] = 0;
            p++;
            // 4: master/slave
            i = strtol(p, &end, 10);
            ASSERT(*end != '\t', EI_SYNTAX);
            ASSERT(i < 0 || i > 1, EINVAL);
            if (i)
            {
                s->flags = IECF_S_MASTER;
            }
            else
            {
                s->flags = IECF_S_SLAVE;
            }
            p = end + 1;
            // 5: client/server
            i = strtol(p, &end, 10);
            ASSERT(*end != '\t', EI_SYNTAX);
            ASSERT(i < 0 || i > 1, EINVAL);
            
            if (i) 
            {
                s->flags |= IECF_S_CLIENT;
            }
            else
            {
                s->flags |= IECF_S_SERVER;
            }
            p = end + 1;
            ASSERT(i != 0 && s->ip[0] == 0, EINVAL); /* Check if IP defined for client */
            // 6: options (IEC constants & IP version)
            ASSERT(load_iec_options(&p, &s->is, &s->flags) < 0, EI_OPTIONS);
            // 7: CA list
            while (1)
            {
                i = strtol(p, &end, 10);
                switch (*end)
                {
                case ',':
                case '\t':
                case '\n':
                    j = i;
                    break;
                default:
                    if (*end == '.' && end[1] == '.')
                    {
                        p = end + 2;
                        j = strtol(p, &end, 10);
                        if (*end == ',' || *end == '\t' || *end == '\n')
                        {
                            if (j >= i) break;
                        }
                    }
                    errno = EI_SYNTAX;
                    goto error;
                }
                for (; *end != ',' && *end != '\t' && *end != '\n' && *end != 0; end++);
                for (; i <= j; i++)
                {
                    ASSERT1(i <= 0 || i > IEC_MAX_SECTORS/* || sectors[i - 1].ca == 0*/, EI_SECTOR);
                    if (sectors[i - 1].ca == 0) continue;
                    index = (i - 1) / 32;
                    off = (i - 1) % 32;
                    ASSERT1(s->sectors[index] & (1 << off), EI_SECTOREXIST);
                    s->sectors[index] |= 1 << off;
                    s->count++;
                }
                p = end + 1;
                if (*end != ',') break;
            }
            // 8: allowed IPs (for server socket)
            if ((s->flags & IECF_S_SERVER) != 0 && *p != '\t' && *p != '\n' && *p != 0)
            {
                for (end = p; *end != '\t' && *end != '\n' && *end != 0; end++);
                i = end - p;
                s->allowed = (char*)malloc(i + 1);
                memmove(s->allowed, p, i);
                s->allowed[i] = 0;
            }
            nsockets++;
            s++;
        }
        // next line
        for (; *p != 0 && *p != '\n'; p++) { ; }
        if (*p == 0) break;
    }
    return 0;
error:
    SET_ERROR(ERR_IN_SOCKETS_CFG, line, iec_strerror(errno));
    return -1;
error1:
    SET_ERROR(ERR_IN_SOCKETS_CFG1, line, iec_strerror(errno), i);
    return -1;
}

static const char* opts[] = { "T0", "T1", "T2", "T3", "K", "W", "CA", "IOA", "IPV", "CIR"
#ifdef __HAVE_SSL_SUPPORT__
, "SSL"
#endif
};

static int get_iec_option(char* buf)
{
    int i;
    char* p;
    for (p = buf; *p != 0; p++)
    {
        *p = toupper(*p);
    }

    for (i = 0; i < sizeof(opts) / sizeof(opts[0]); i++)
    {
        if (strcmp(buf, opts[i]) == 0)
        {
            return i;
        }
    }

    return -1;
}

static int load_iec_options(char** bufptr, iec_settings_t* is, int* flags)
{
    char* p = *bufptr;
    char* end;
    char* eq;
    int i, j;
    char buf[32];
    while (*p != 0 && *p != '\t')
    {
        while (*p == ' ') p++;
        for (end = p, eq = NULL; *end != 0 && *end != '\t' && *end != ','; end++)
            if (*end == '=')
                eq = end;
        if (end - p >= 32) return -1;
        i = eq == NULL ? end - p : eq - p;
        memmove(buf, p, i);
        buf[i] = 0;
        i = get_iec_option(buf);
        if (i < 0) return -1;
        if (eq == NULL)
        {
            return -1;
        }
        else
        {
            j = strtol(eq + 1, &p, 10);
            if (p != end || j < 0) return -1;
            switch (i)
            {
            case 0:
                if (j > 255) return -1;
                is->t0 = j;
                break;
            case 1:
                if (j > 255) return -1;
                is->t1 = j;
                break;
            case 2:
                if (j > 255) return -1;
                is->t2 = j;
                break;
            case 3:
                if (j > 255) return -1;
                is->t3 = j;
                break;
            case 4:
                if (j > 255) return -1;
                is->k = j;
                break;
            case 5:
                if (j > 255) return -1;
                is->w = j;
                break;
            case 6:
                if (j > 2) return -1;
                is->ca_len = j;
                break;
            case 7:
                if (j > 3) return -1;
                is->ioa_len = j;
                break;
            case 8:
                if (j != 4 && j != 6) return -1;
                if (j == 6) *flags |= IECF_S_IPV6;
                break;
            case 9:
                if (j != 0 && j != 1) return -1;
                if (j) *flags |= IECF_S_IC_REQ;
                break;
            case 10:
#ifdef __HAVE_SSL_SUPPORT__
                if (j != 0 && j != 1) return -1;
                if (j) *flags |= IECF_S_SSL;
#else
                return -1;
#endif
                break;
            }
        }
        p = end;
        if (*end == ',') p++;
    }
    *bufptr = p;
    return 0;
}

/*
 * Load IEC sectors information from DATA buffer
 *
 * Return 0 if success, otherwise -1
 */
static int load_sectors(const char* buffer)
{
    char* p = NULL, * end = NULL;
    int flag, ca, ioa, type, l_ca, l_ioa, line;
    sector_t* s = NULL;

    for (p = (char*)buffer, line = 1; *p != 0; p++, line++)
    {
        switch (*p)
        {
        case '\n':
        case '#':
            break;
        default:
            // 1: flag
            flag = strtol(p, &end, 10);
            if (*end != '\t' || flag < 0 || flag > 2)
            {
                errno = EI_SYNTAX;
                return line;
            }
            if (flag == 0)
            {
                break;
            }
            p = end + 1;
            // 2: CA
            ca = strtol(p, &end, 10);
            if (*end != '\t')
            {
                errno = EI_SYNTAX;
                return line;
            }
            if (ca <= 0 || ca > IEC_MAX_SECTORS)
            {
                errno = EI_SECTOR;
                return line;
            }
            s = &sectors[ca - 1];
            if (s->ca == 0)
            {
                s->ca = ca;
                nsectors++;
                if (ca > maxsector)
                {
                    maxsector = ca;
                }
            }
            p = end + 1;
            // 3: IOA
            ioa = strtol(p, &end, 10);
            if (*end != '\t')
            {
                errno = EI_SYNTAX;
                return line;
            }
            if (ioa < 1 || ioa > IEC_MAX_IOA)
            {
                errno = EI_IOA;
                return line;
            }
            s->count++;
            if (ioa > s->max)
            {
                s->max = ioa;
            }
            p = end + 1;
            // 4: type
            type = strtol(p, &end, 10);
            if (*end != '\t')
            {
                errno = EI_TYPE;
                return line;
            }
            if (type < 1 || type > IECT_MAX)
            {
                errno = EI_TYPE;
                return line;
            }
            p = end + 1;
            // 5,6: additional CA, IOA
            switch (read_linked_addr(&p, &l_ca, &l_ioa))
            {
            case -1:
                return line;
            case 1:
                if (l_ca == 0)
                {
                    l_ca = ca;
                }
                if (l_ioa == 0)
                {
                    l_ioa = ioa;
                }
                s = &sectors[l_ca - 1];
                if (s->ca == 0)
                {
                    s->ca = l_ca;
                    nsectors++;
                    if (l_ca >= maxsector)
                    {
                        maxsector = l_ca;
                    }
                }
                s->count++;
                if (l_ioa > s->max)
                {
                    s->max = l_ioa;
                }
            }
            break;
        }
        
        // next line
        for (; *p != 0 && *p != '\n'; p++) { ; }

        if (*p == 0)
        {
            break;
        }
    }

    return 0;
}

static void free_allowed_ip()
{
    int i;
    socket_t* s;
    for (i = nsockets - 1, s = sockets; i >= 0; i--, s--)
    {
        if (s->allowed != NULL) {
            free(s->allowed);
            s->allowed = NULL;
        }
    }
}

/*
 * Load all data from memory buffers
 */
int iec_load_config_from_mem(const char* sockets_buf, const char** data_bufs, const iec_settings_t* is, syncobj_t* syncobj)
{
    char* end = NULL;
    const char* p = NULL;
    iec_settings_t ist;
    int i, j, k, line, flag, ca, ioa, l_ca, l_ioa, type, initialized = 0;
    uint32_t m = 0;

    if (is == NULL)
    {
        memset(&ist, 0, sizeof(iec_settings_t));
    }
    else
    {
        memmove(&ist, is, sizeof(iec_settings_t));
    }

    nsectors = nsockets = maxsector = 0;
    char* tmp = (char*)calloc(1, IEC_MAX_SECTORS * sizeof(sector_t) + IEC_MAX_SOCKETS * sizeof(socket_t));
    if (tmp == NULL)
    {
        SET_ERROR(ERR_INIT, errno, strerror(errno));
        return -1;
    }

    sectors = (sector_t*)tmp;
    sockets = (socket_t*)(tmp + IEC_MAX_SECTORS * sizeof(sector_t));

    if (status_sector)
    {
        sectors[status_sector - 1].ca = status_sector;
        maxsector = status_sector;
    }

    for (i = 0; data_bufs[i] != NULL; i++)
    {
        if ((line = load_sectors(data_bufs[i])) != 0)
        {
            SET_ERROR(ERR_IN_DATA_CFG, i + 1, line, iec_strerror(errno));
            free(tmp);
            return -1;
        }
    }

    if (status_sector)
    {
        sectors[status_sector - 1].count = maxsector;
        sectors[status_sector - 1].max = maxsector;
    }

    if (load_sockets((char*)sockets_buf, &ist) < 0)
    {
        free_allowed_ip();
        free(tmp);
        return -1;
    }

    if (iec_init(maxsector, nsockets, &ist, syncobj) < 0)
    {
        free_allowed_ip();
        free(tmp);
        return -1;
    }

    initialized = 1;
    for (i = 0; i < IEC_MAX_SECTORS; i++)
    {
        if (sectors[i].ca)
        {
            if (sectors[i].count == 0)
            {
                sectors[i].ca = 0;
            }
            else
            {
                if (iec_sector_new(sectors[i].ca, sectors[i].count, sectors[i].max) < 0)
                {
                    goto error1;
                }
            }
        }
    }

    for (i = 0; i < nsockets; i++)
    {
        if ((sockets[i].h_socket = iec_socket_new(
            (sockets[i].flags & IECF_S_CLIENT) ? sockets[i].ip : NULL,
            sockets[i].port, sockets[i].count, sockets[i].flags, &sockets[i].is)) < 0)
            goto error1;
        if (sockets[i].allowed != NULL && iec_socket_set_allowed_list(sockets[i].h_socket,
            sockets[i].allowed) < 0) {
            errno = EI_SYNTAX;
            goto error1;
        }
        for (j = 0; j < IEC_MAX_SECTORS / 32; j++)
        {
            if ((m = sockets[i].sectors[j]) != 0)
            {
                for (k = 0; k < 32; k++)
                {
                    if (m & (1 << k))
                    {
                        ca = j * 32 + k + 1;
                        if (iec_socket_add_sector(sockets[i].h_socket, ca) < 0) goto error1;
                    }
                }
            }
        }
    }

    if (status_sector)
    {
        iec_sector_t* s_sector = IEC_SECTOR(status_sector);
        if (s_sector == NULL)
        {
            errno = EINVAL;
            goto error;
        }
        iec_data_t* data;
        for (i = 0; i < IEC_MAX_SECTORS; i++)
        {
            if (sectors[i].ca) {
                if (iec_sector_add_info(status_sector, i + 1, IECT_STEP, 0) < 0) goto error;
            }
        }
        for (i = 0, data = s_sector->data; i < s_sector->count; i++, data++)
        {
            iec_sector_t* sector = IEC_SECTOR(data->ioa);
            if (sector == NULL)
            {
                continue;
            }
            data->value.int8 = IECF_SECTOR_ERROR;
            if (sector->master >= 0) data->value.int8 |= IECF_SECTOR_IN;
            if (sector->n_slaves > 0) data->value.int8 |= IECF_SECTOR_OUT;
        }
    }
    free_allowed_ip();
    free(tmp);

    tmp = NULL;
    int bn;
    for (bn = 0; data_bufs[bn] != NULL; bn++)
        for (p = data_bufs[bn], line = 1; *p != 0; p++, line++)
        {
            switch (*p)
            {
            case '#':
            case '\n':
                break;
            default:
                // 1: flag
                flag = strtol(p, &end, 10);
                ASSERT(*end != '\t' || flag < 0 || flag > 1, EI_SYNTAX);
                if (flag == 0) break;
                p = end + 1;
                // 2: CA
                ca = strtol(p, &end, 10);
                p = end + 1;
                // 3: IOA
                ioa = strtol(p, &end, 10);
                p = end + 1;
                // 4: value type
                i = strtol(p, &end, 10);
                ASSERT(*end != '\t', EI_SYNTAX);
                ASSERT((type = valid_type(i)) == 0, EI_TYPE);
                p = end + 1;
                if (iec_sector_add_info(ca, ioa, type, 0) < 0) goto error;
                // 5,6: additional CA, IOA
                i = read_linked_addr((char**)&p, &l_ca, &l_ioa);
                if (i < 0) goto error;
                if (i > 0) {
                    if (l_ca == 0) l_ca = ca;
                    if (l_ioa == 0) l_ioa = ioa;
                    if (iec_object_duplicate(ca, ioa, l_ca, l_ioa) < 0) goto error;
                }
            }
            for (; *p != 0 && *p != '\n'; p++);
            if (*p == 0) break;
        }
    return 0;
error:
    SET_ERROR(ERR_IN_DATA_CFG, bn + 1, line, iec_strerror(errno));
error1:
    if (initialized) iec_free();
    if (tmp) {
        free_allowed_ip();
        free(tmp);
    }

    return -1;
}

/*
 * Load all data from files
 */
int iec_load_config_from_files(const char* path, const char* sockets_file, const char** data_files, const iec_settings_t* is, syncobj_t* syncobj)
{
    if (path == NULL || sockets_file == NULL || data_files == NULL)
    {
        errno = EINVAL;
        return -1;
    }

    char filename[256] = {0};
    int i = 0, j = 0, n = 0;

    for (n = 0; data_files[n] != NULL; n++)
    {
        ;
    }

    if (n == 0)
    {
        errno = EINVAL;
        return -1;
    }

    if (*sockets_file == '/' || *sockets_file == '.')
    {
        strncpy(filename, sockets_file, sizeof(filename));
    }
    else
    {
        snprintf(filename, sizeof(filename), "%s/%s", path, sockets_file);
    }

    filename[sizeof(filename) - 1] = 0;
    char* sockets_buf = load_file(filename);

    if (sockets_buf == NULL)
    {
        SET_ERROR(ERR_LOAD_FILE, sockets_file, iec_strerror(errno));
        return -1;
    }

    char** data = (char**)malloc((n + 1) * sizeof(char*));
    for (i = 0; i < n; i++)
    {
        if (data_files[i][0] == '/' || data_files[i][0] == '.')
        {
            strncpy(filename, data_files[i], sizeof(filename));
        }
        else
        {
            snprintf(filename, sizeof(filename), "%s/%s", path, data_files[i]);
        }

        if (NULL == data)
        {
            errno = EACCES;
            return -1;
        }

        filename[sizeof(filename) - 1] = 0;
        data[i] = load_file(filename);
        if (data[i] == NULL)
        {
            SET_ERROR(ERR_LOAD_FILE, data_files[i], iec_strerror(errno));
            for (j = i - 1; j >= 0; j--)
            {
                free(data[j]);
            }

            free(data);
            free(sockets_buf);
            return -1;
        }
    }
    
    data[n] = NULL;
    int status = iec_load_config_from_mem(sockets_buf, (const char**)data, is, syncobj);
    for (i = n - 1; i >= 0; i--)
    {
        free(data[i]);
    }

    free(data);
    free(sockets_buf);

    return status;
}


/*
****************************************************************************************************************
*= = 文件结束
****************************************************************************************************************
*/


